#include <time.h>

#include <math.h>
#include <fcntl.h>
#include <stdio.h>
#include <unistd.h>
#include <string.h>
#include <sys/time.h>
#include "posemath.h"
#include "rtapi/rtapi.h"
#include "hal/hal.h"
#include "motion.h"
#include "mot_priv.h"
#include "rtapi/rtapi_math.h"
#include "motion_debug.h"
#include "config.h"
#include "nml_intf/motion_types.h"
#include "errcode.h"
#include "hal/OsDeclarations.h"
#include "axis/servoError.h"
#include "NOF.h"
#include "dyna/jaka_dyn_intf.h"
#include "jacobin.h"
#include "jacobin.h"
#include "motion/semintf.h"
#include "zuc/nml_intf/zucpos.h"
#include "nml_intf/zucpose.h"
#include "nml_intf/pnio_mem.h"
#include "nml_intf/eipio_mem.h"
#include "motion/semintf.h"
#include "motionStrategyType.h"
#include "controlDefine.h"
//#include "tp/tpmanager.hh"
#include "remoteApi/usrRemote.h"
#include "IOmapping.h"
#include "dynamic/identify_dynamics.h"
#include "timemonitor.h"
#include "motion/semintf.h"
#include "torque_control_queue.h"
#include "funcio/funcio.h"
#include "safety/io/safeio.h"
#include "rtdev/rtdev.h"
#include "rtdev/bp/bp_intf.h"
#include "rtdev/safetyboard/scbitf.h"
#include "rtdev/tio_intf_c.h"
#include "axis/axis.h"
#include "axisgroup/axisgroup.h"
#include "kine/kine.h"
#include "safety/io/safeio.h"
#include "dyna/jaka_dyn_intf.h"
#include "axisgroup/axisgroup_robot.h"
#include "axisgroup/robot_mgr.h"

//#define MAX_FILTER_SIZE	20
#define ON 1
#define OFF 0
#define SAFETY_DI_ON 2              // 上升沿激活的安全功能使用了1和2来判定上升沿，遂使用此宏定义
#define SAFETY_DI_OFF 1             // 上升沿激活的安全功能使用了1和2来判定上升沿，遂使用此宏定义
#define PRE_CHECK_STOP_POS_ACC 500  //位置超限预判后降速时采用的线加速度,避免MC承受不住较大加速度而掉使能
#define PRE_CHECK_STOP_ORI_ACC 300  //位置超限预判后降速时采用的姿态加速度,避免MC承受不住较大加速度而掉使能

// 功能IO在手自动模式下触发时的报错标志位
#define PROG_STOP_BIT 0x0001
#define PROG_RUN_BIT 0x0002
#define PROG_PAUSE_BIT 0x0004
#define PROG_RESUME_BIT 0x0008
#define POWER_OFF_BIT 0x0010
#define POWER_ON_BIT 0x0020
#define SERVO_ON_BIT 0x0040
#define SERVO_OFF_BIT 0x0080
#define BACK_TO_INIT_POS_BIT 0x0100
#define PERCENTAGE_MODE_L1_BIT 0x0200
#define PERCENTAGE_MODE_L2_BIT 0x0400
#define PROTECTIVE_STOP_BIT 0x0800
#define CLEAR_ERROR_BIT 0x1000
#define ENABLE_DRAG_BIT 0x2000
#define DISABLE_DRAG_BIT 0x4000

/* global kinematics flags */
KINEMATICS_FORWARD_FLAGS ctrl_fflags_flange = 0;
KINEMATICS_INVERSE_FLAGS ctrl_iflags_flange = 0;

KINEMATICS_FORWARD_FLAGS ctrl_fflags = 0;
KINEMATICS_INVERSE_FLAGS ctrl_iflags = 0;

/* 1/servo cycle time */
double servo_freq = 125.0;
static TIOStatus tioStatus = {0};
static double momvelscaleLast = 1.0;
static int collisionStateLast[ZUCMOT_MAX_JOINTS] = {0};
int collisionStateCurr[ZUCMOT_MAX_JOINTS] = {0};
static int collisionTimeOut = 0;
static int collision_detected_in_movement = 0;
// static double home_pos[ZUCMOT_MAX_JOINTS] = {0.0, 90.0, 0.0, 90.0, 180.0, 0.0, 0};
static int send_enable_signal = 0;
static int enable_complete = 0;
static int enable_last_stat = 0;
static double last_posbf_before_enable[ZUCMOT_MAX_JOINTS] = {0.0};
static FILE* payload_identify_file = NULL;
// static int cycleNum = 0;	// for debug
// static ServoMoveData servoMoveData;
ZucPose last_carte_pos_cmd;
int cycleCount;
int queryServoVersionTimeOut[ZUCMOT_MAX_JOINTS] = {0};
static int cur_cmd[ZUCMOT_MAX_JOINTS] = {0};
static int grv_cur_cmd[ZUCMOT_MAX_JOINTS] = {0};
static double externFtBsensor[CART_DIM] = {0.0};
static double target_jrk[ZUCMOT_MAX_JOINTS] = {0.0};
static double target_acc[ZUCMOT_MAX_JOINTS] = {0.0};
static double target_vel[ZUCMOT_MAX_JOINTS] = {0.0};
static double target_pos[ZUCMOT_MAX_JOINTS] = {0.0};
int32_u zucint32_u;
static double target_pos_pre[ZUCMOT_MAX_JOINTS] = {0.0};
static ZucPose worldLast_admit_revised;
static int last_compliant_stat = 0;

static int first_case3 = 1;
static bool safeZone_reduce_stop = 1;
static bool safeZone_reduce_mode = 1;
static bool safeZone_effect = 1;
static int three_position_enabling_exit_drag_req = 0;
static int estop_motion_plan_flag = 0;
double target_z;
int stopping = 0;
int wait_robot_power_on = 0;

static double last_drag_y[ZUCMOT_MAX_JOINTS] = {};
static double last_drag_x[ZUCMOT_MAX_JOINTS] = {};

// #define WRITE_JOINT_FILE
#ifdef WRITE_JOINT_FILE
static int file_first = 0;
static FILE* joint1_pos = NULL;
static FILE* joint2_pos = NULL;
static FILE* joint3_pos = NULL;
static FILE* joint4_pos = NULL;
static FILE* joint5_pos = NULL;
static FILE* joint6_pos = NULL;
static FILE* joint7_pos = NULL;
static FILE* joint1_vel = NULL;
static FILE* joint2_vel = NULL;
static FILE* joint3_vel = NULL;
static FILE* joint4_vel = NULL;
static FILE* joint5_vel = NULL;
static FILE* joint6_vel = NULL;
static FILE* joint7_vel = NULL;
static FILE* consumeTime = NULL;

static FILE* linearVel_file = NULL;
static FILE* Angular_Vel_file = NULL;
static FILE* Attitude_aix = NULL;
static FILE* rotationVel_file = NULL;
static FILE* pos_x_file = NULL;
static FILE* pos_y_file = NULL;
static FILE* pos_z_file = NULL;
static FILE* cycle_counter_file = NULL;
static double toppra_tor[ZUCMOT_MAX_JOINTS] = {0};
#endif
double pos_error;

// static FILE *position_file = NULL;

// #define WATCH_FLAGS 1
//  extern int nWrite_file_flag;	//WH6 为保存各关节数据 在tp.cpp中定义
//  extern int nFile_first;		//WH6 首次保存数据   在tp.cpp中定义
//  static FILE *Joint_position_file = NULL; //WH6 关节位置
//  static FILE *Joint_velocity_file = NULL; //WH6 关节速度

/***********************************************************************
*                  LOCAL VARIABLE DECLARATIONS                         *
************************************************************************/

static unsigned long last_period = 0;
static double servo_period = 0.008;
// static int  bHitLimit = 0;
static int bHitSingularity = 0;
static int singularity_check_flag = 0;

// static const double REV2RAD = 6.283185308;
// static const double ABSFULLCNT = 524288;
// static const double DEG2ABSCNT = 1456.35556;  //2^18/180

#define ACC_ANGLE_LMT 720 * 0.008 * 0.008
#define VEL_ANGLE_LMT 120 * 0.008
#define ACC_LMT 3000 * 0.008 * 0.008
#define VEL_LMT 1000 * 0.008
#define VAR_NUM 2
#define CONS_NUM 5

// ROBOT_IS_MOVING 含义是指令是否在插补，而不是机器人实际位置是否在变换
#define ROBOT_IS_MOVING                                                                                                                 \
    ((fabs(joints[0].vel_cmd) + fabs(joints[1].vel_cmd) + fabs(joints[2].vel_cmd) + fabs(joints[3].vel_cmd) + fabs(joints[4].vel_cmd) + \
      fabs(joints[5].vel_cmd) + fabs(joints[6].vel_cmd)) > 2 ?                                                                          \
         1 :                                                                                                                            \
         0)
/***********************************************************************
*                      LOCAL FUNCTION PROTOTYPES                       *
************************************************************************/

/* the following functions are called (in this order) by the main
   controller function.  They are an attempt to break the huge
   function (originally 1600 lines) into something a little easier
   to understand.
*/

/* 'process_inputs()' is responsible for reading hardware input
   signals (from the HAL) and doing basic processing on them.  In
   the case of position feedback, that means removing backlash or
   screw error comp and calculating the following error.  For 
   switches, it means debouncing them and setting flags in the
   zucmotStatus structure.
*/
static void process_inputs(void);

/* 'handle_jjogwheels()' reads jogwheels, decides if they should be
   enabled, and if so, changes the free mode planner's target position
   when the jogwheel(s) turn.
*/
static void handle_jjogwheels(void);
static void handle_ajogwheels(void);

/* 'get_pos_cmds()' generates the position setpoints.  This includes
   calling the trajectory planner and interpolating its outputs.
*/
static void get_pos_cmds();

/* 'handle_servo_error()' will detect the collision and do corresponding 
	work on the motion controller level, and interact with can_usr module.
*/
static void handle_servo_error();

/* 'handle_drag_req()' will detect the drag mode request from physical 
    button
 */
static void handle_drag_req();

static void handle_point_button_req();

static void handle_category_one_stop();

/* 'get_curr_cmds()' generates the current command for each joint.  This 
	includes calling the gravity and friction compensation calculation.
*/
static void get_curr_cmds();

static void motAbortMotion();

static int posCmdInRange(double* joint_cmds, double* curr_joints, int* jnt_num);

/* 'output_to_hal()' writes the handles the final stages of the
   control function.  It applies screw comp and writes the
   final motor position to the HAL (which routes it to the PID
   loop).  It also drives other HAL outputs, and it writes a
   number of internal variables to HAL parameters so they can
   be observed with halscope and halmeter.
*/
static void output_to_hal(void);

/* 'update_status()' copies assorted status information to shared
   memory (the zucmotStatus structure) so that it is available to
   higher level code.
*/
static void update_status(void);

static void update_servo_enable_delay();

static bool any_joint_in_fault(void);

static bool any_unhandled_error(void);

static int kinematicsInverse(const ZucPose* toolOffset,
                             const ZucPose* world,
                             double* joint,
                             const unsigned long* interp_iflags,
                             unsigned long* interp_fflags,
                             const PmRpy* base_offset,
                             const ZucPose* user_offset,
                             uint8_t grp_id = 0)
{
    RobJointVal jpos(joint);
    kine::KineItf::kine_Conf conf;
    int ret = axisgroup::AxisGroupMgr::instance().get_axisgroup(grp_id)->kine()->ikine(*world, jpos, conf, toolOffset, user_offset, base_offset);
    jpos.array_value(joint);
    return ret;
}
static int kinematicsForward(const ZucPose* toolOffset,
                             const double* joint,
                             ZucPose* world,
                             const unsigned long* interp_fflags,
                             unsigned long* interp_iflags,
                             const PmRpy* base_offset,
                             const ZucPose* user_offset,
                             uint8_t grp_id = 0)
{
    int ret = -1;
    RobJointVal jpos(joint);
    ZucPose pose;
    ret = axisgroup::AxisGroupMgr::instance().get_axisgroup(grp_id)->kine()->fkine(jpos, pose, toolOffset, user_offset, base_offset);
    *world = pose;
    return ret;
}

//控制器拖拽速度限制
static void drag_tcp_speed_limit(void)
{
    if (zucmotDebug->dragging == 0)
    {
        return;
    }

    if ((zucmotStatus->vel_tcp_linear - zucmotConfig->safetyCfg.dragTcpSpeedLimit) > 0.001)
    {
        char str[256];
        snprintf(str, 256, "{\"fixed_key\":[\"%.3f\",\"%.3f\"]}", zucmotConfig->safetyCfg.dragTcpSpeedLimit, zucmotStatus->vel_tcp_linear);
        reportError(SCB_ERR_DRAG_OVER_SPEED, "scb error occured, error code is 0x%x  # %s ", SCB_ERR_DRAG_OVER_SPEED, str);
        if (GET_MOTION_DRAG_FLAG())
        {
            zucmotDebug->dragging = 0;  // only for miniCobo
        }
    }
}

// static void print_pose(const char* label, ZucPose cartPos)
// {
// 	rtapi_print("%s: x=%f, y=%f, z=%f, a=%f, b=%f, c=%f\n", label,
// 		 cartPos.tran.x, cartPos.tran.y, cartPos.tran.z, cartPos.a, cartPos.b, cartPos.c);
// }

static void check_capture(void)
{
    // TODO:将捕获功能迁移到新的功能DI模块中

    // CaptureSettings* pStateCapt;
    // int j_num, latest_di_stat = 0;

    // for (int chn = 0; chn < 8; chn++)
    // {
    //     pStateCapt = &(zucmotConfig->captureSettings[chn]);
    //     if (zucmotStatus->captureStatus[chn].captStatus == 1)
    //     {
    //         if (pStateCapt->trigger.ioType == 0)
    //         {
    //             if (zucmotConfig->cab_type == 1)
    //             {
    //                 latest_di_stat = (zucmot_hal_data->physical_di[pStateCapt->trigger.ioIndex]);
    //             }
    //             else if (zucmotConfig->cab_type == 2)
    //             {
    //                 SCBUserDI user_di_signal = get_scb_user_DI_status();
    //                 if (pStateCapt->trigger.ioIndex < 8)
    //                 {
    //                     latest_di_stat = user_di_signal.b[pStateCapt->trigger.ioIndex];
    //                 }
    //             }
    //             if (pStateCapt->trigger.ioIndex == 0)
    //             {
    //                 rtapi_print("captureStatus[0]-->synch_di[0] = %d, user_di_1 = %d\n", zucmotStatus->synch_di[pStateCapt->trigger.ioIndex], latest_di_stat);
    //             }
    //             if (pStateCapt->trigger.trigEvent == 0 && zucmotStatus->synch_di[pStateCapt->trigger.ioIndex] && !latest_di_stat)
    //             {
    //                 if (pStateCapt->stateType == 0)
    //                 {
    //                     zucmotStatus->captureStatus[chn].captRes.cartePose = zucmotStatus->carte_pos_cmd;
    //                 }
    //                 else if (pStateCapt->stateType == 1)
    //                 {
    //                     for (j_num = 0; j_num < zucmotConfig->numJoints; j_num++)
    //                     {
    //                         zucmotStatus->captureStatus[chn].captRes.jointPos[j_num] = joints[j_num].pos_cmd;
    //                     }
    //                 }
    //                 zucmotStatus->captureStatus[chn].captStatus = 2;
    //                 rtapi_print("falling edge captured: %f, %f, %f, %f, %f, %f\n",
    //                             zucmotStatus->carte_pos_cmd.tran.x,
    //                             zucmotStatus->carte_pos_cmd.tran.y,
    //                             zucmotStatus->carte_pos_cmd.tran.z,
    //                             zucmotStatus->carte_pos_cmd.a,
    //                             zucmotStatus->carte_pos_cmd.b,
    //                             zucmotStatus->carte_pos_cmd.c);
    //                 for (j_num = 0; j_num < zucmotConfig->numJoints; j_num++)
    //                 {
    //                     rtapi_print("falling edge captured joint %d: %f\n", j_num, joints[j_num].pos_cmd);
    //                 }
    //             }
    //             else if (pStateCapt->trigger.trigEvent == 1 && !zucmotStatus->synch_di[pStateCapt->trigger.ioIndex] && latest_di_stat)
    //             {
    //                 if (pStateCapt->stateType == 0)
    //                 {
    //                     zucmotStatus->captureStatus[chn].captRes.cartePose = zucmotStatus->carte_pos_cmd;
    //                 }
    //                 else if (pStateCapt->stateType == 1)
    //                 {
    //                     for (j_num = 0; j_num < zucmotConfig->numJoints; j_num++)
    //                     {
    //                         zucmotStatus->captureStatus[chn].captRes.jointPos[j_num] = joints[j_num].pos_cmd;
    //                     }
    //                 }
    //                 zucmotStatus->captureStatus[chn].captStatus = 2;
    //                 rtapi_print("rising edge captured: %f, %f, %f, %f, %f, %f\n",
    //                             zucmotStatus->carte_pos_cmd.tran.x,
    //                             zucmotStatus->carte_pos_cmd.tran.y,
    //                             zucmotStatus->carte_pos_cmd.tran.z,
    //                             zucmotStatus->carte_pos_cmd.a,
    //                             zucmotStatus->carte_pos_cmd.b,
    //                             zucmotStatus->carte_pos_cmd.c);
    //                 for (j_num = 0; j_num < zucmotConfig->numJoints; j_num++)
    //                 {
    //                     rtapi_print("rising edge captured joint %d: %f\n", j_num, joints[j_num].pos_cmd);
    //                 }
    //             }
    //         }
    //     }
    // }
}

extern double coef_lpf;

extern void get_tar_pos(const double left_theta, ZucPose* tar_pos)
{
    if (zucmotStatus->convyr_type == 2)
    {
        rtapi_print(" ######################## [get_tar_pos] zucmotStatus->last_theta = %f\n", zucmotStatus->last_theta);
        PmRpy rpy_desired, delta_rpy, usr_to_world_rpy, tar_rpy;
        PmRotationMatrix rot_desired, delta_rot, usr_to_world_rot;
        PmHomogeneous desired_to_usr_t, desired_to_o_t, delta_t_to_o, usr_to_o_t, o_to_usr_t, usr_to_world_t, world_to_o_t, temp_t1, tar_t;
        //获得当前TCP相对于当前用户坐标系的齐次矩阵,用以计算r_for_convyr
        rpy_desired.r = zucmotStatus->disable_convyr_real_pos.c * PM_PI / 180.0;  //现在desired_pos已是实际位置
        rpy_desired.p = zucmotStatus->disable_convyr_real_pos.b * PM_PI / 180.0;
        rpy_desired.y = zucmotStatus->disable_convyr_real_pos.a * PM_PI / 180.0;
        pmRpyMatConvert(&rpy_desired, &rot_desired);
        desired_to_usr_t.rot = rot_desired;
        desired_to_usr_t.tran = zucmotStatus->disable_convyr_real_pos.tran;

        //获得当前用户坐标系相对于传送带旋转圆心坐标系{O}的变换：T{user to O} = T{world to O}*T{user to world}
        usr_to_world_rpy.r = zucmotConfig->user_offset.c * PM_PI / 180.0;
        usr_to_world_rpy.p = zucmotConfig->user_offset.b * PM_PI / 180.0;
        usr_to_world_rpy.y = zucmotConfig->user_offset.a * PM_PI / 180.0;
        pmRpyMatConvert(&usr_to_world_rpy, &usr_to_world_rot);
        usr_to_world_t.rot = usr_to_world_rot;
        usr_to_world_t.tran = zucmotConfig->user_offset.tran;
        pmHomInv(&zucmotStatus->o_to_world, &world_to_o_t);
        pmHomHomMult(&world_to_o_t, &usr_to_world_t, &usr_to_o_t);
        pmHomInv(&usr_to_o_t, &o_to_usr_t);
        //获得当前TCP相对于传送带圆心坐标系的齐次矩阵,用以计算r_for_convyr
        pmHomHomMult(&usr_to_o_t, &desired_to_usr_t, &desired_to_o_t);

        //获得相对于{O}的变换，比较简单，只是绕z轴的旋转
        delta_rpy.r = left_theta;  //考虑速度突变
        delta_rpy.p = 0.0;
        delta_rpy.y = 0.0;
        pmRpyMatConvert(&delta_rpy, &delta_rot);
        delta_t_to_o.rot = delta_rot;
        delta_t_to_o.tran.x = 0.0;
        delta_t_to_o.tran.y = 0.0;
        delta_t_to_o.tran.z = 0.0;

        pmHomHomMult(&delta_t_to_o, &desired_to_o_t, &temp_t1);
        pmHomHomMult(&o_to_usr_t, &temp_t1, &tar_t);
        tar_pos->tran = tar_t.tran;
        pmMatRpyConvert(&(tar_t.rot), &tar_rpy);
        if (zucmotStatus->rotate_tool == 1)
        {
            tar_pos->a = tar_rpy.y / PM_PI * 180.0;
            tar_pos->b = tar_rpy.p / PM_PI * 180.0;
            tar_pos->c = tar_rpy.r / PM_PI * 180.0;  //退出传送带跟踪的减速段姿态也变
        }
        else
        {
            tar_pos->a = zucmotStatus->disable_convyr_real_pos.a;  //退出传送带跟踪的减速段姿态不变
            tar_pos->b = zucmotStatus->disable_convyr_real_pos.b;
            tar_pos->c = zucmotStatus->disable_convyr_real_pos.c;
        }
    }
    else
    {
        rtapi_print(" zucmotStatus->convyr_type == %d ######################## [get_tar_pos] zucmotStatus->last_theta = %f\n",
                    zucmotStatus->convyr_type,
                    zucmotStatus->last_theta);
        //compute target pos when disable linear conveyor
        tar_pos->tran.x = zucmotStatus->disable_convyr_real_pos.tran.x + zucmotStatus->conveyorDir.x * left_theta;
        tar_pos->tran.y = zucmotStatus->disable_convyr_real_pos.tran.y + zucmotStatus->conveyorDir.y * left_theta;
        tar_pos->tran.z = zucmotStatus->disable_convyr_real_pos.tran.z + zucmotStatus->conveyorDir.z * left_theta;
        tar_pos->a = zucmotStatus->disable_convyr_real_pos.a;
        tar_pos->b = zucmotStatus->disable_convyr_real_pos.b;
        tar_pos->c = zucmotStatus->disable_convyr_real_pos.c;
    }
}

extern void get_vel_theta(double* vel_for_convyr, double* theta)
{
    double max_for_acc = zucmotConfig->synch_acc, max_for_dec = zucmotConfig->synch_acc;
    double trans_from_encoder = 0.0, omega = 0.0, temp_vel = 0.0, vel_from_encoder = 0.0;
    static double last_trans_from_encoder = 0.0;
    trans_from_encoder =
        (zucmotStatus->uhiData.pulses - zucmotStatus->conveyorOrigin) /**0.00001*/ * zucmotStatus->conveyorPulseEquivalent;  //绕{O}的z轴转过的弧度, rad
    if (zucmotStatus->first_cycle_in_tracking)
    {
        last_trans_from_encoder = 0.0;
    }
    vel_from_encoder = (trans_from_encoder - last_trans_from_encoder) * (1.0 / zucmotConfig->trajCycleTime) * zucmotStatus->r_for_convyr;
    // rtapi_print("[get_vel_theta]  --->   trans_from_encoder = %f, zucmotStatus->r_for_convyr = %f, vel_from_encoder = %f\n", trans_from_encoder, zucmotStatus->r_for_convyr, vel_from_encoder);

    if (zucmotStatus->convyr_type == 1)
    {
        zucmotStatus->r_for_convyr = 1.0;
    }
    if (fabs(zucmotStatus->r_for_convyr) < Q_FUZZ)
    {
        *vel_for_convyr = zucmotStatus->last_vel_for_convy;
        *theta = trans_from_encoder;
        return;
    }

    switch (zucmotStatus->status_acc_convyr)
    {
    case 0:
        /* accelerate */
        // rtapi_print(" -------------------------------- enter [0] --------------------------------\n");
        *vel_for_convyr = zucmotStatus->last_vel_for_convy + max_for_acc * zucmotConfig->trajCycleTime;  // unit: mm/s
        if (vel_from_encoder != 0 && *vel_for_convyr > 1.5 * vel_from_encoder)
        {
            *vel_for_convyr = 1.5 * vel_from_encoder;
        }
        omega = *vel_for_convyr / zucmotStatus->r_for_convyr;
        *theta = zucmotStatus->last_theta + omega * zucmotConfig->trajCycleTime;
        // rtapi_print("vel_for_convyr = %f, theta = %f\n", *vel_for_convyr, *theta);
        if (*theta >= trans_from_encoder && !zucmotStatus->first_cycle_in_tracking)
        {
            *vel_for_convyr = (trans_from_encoder - zucmotStatus->last_theta) * (1.0 / zucmotConfig->trajCycleTime) * zucmotStatus->r_for_convyr;
            // rtapi_print("[*theta > trans_from_encoder] ---> *vel_for_convyr = %f, zucmotStatus->last_vel_for_convy = %f\n", *vel_for_convyr, zucmotStatus->last_vel_for_convy);
            if (*vel_for_convyr < zucmotStatus->last_vel_for_convy)
            {
                if (fabs(zucmotStatus->last_vel_for_convy - *vel_for_convyr) * (1.0 / zucmotConfig->trajCycleTime) > max_for_acc)
                {
                    *vel_for_convyr = zucmotStatus->last_vel_for_convy - max_for_acc * zucmotConfig->trajCycleTime;
                    omega = *vel_for_convyr / zucmotStatus->r_for_convyr;
                    *theta = zucmotStatus->last_theta + omega * zucmotConfig->trajCycleTime;
                    zucmotStatus->status_acc_convyr = 1;  // begin decelerate
                    // rtapi_print("[*vel_for_convyr < last_vel_for_convy] && [over max_acc, acc = %f]   --->  recalculate *vel_for_convyr = %f, *theta = %f, [---1]\n",
                    // 				fabs(zucmotStatus->last_vel_for_convy - *vel_for_convyr) / zucmotConfig->trajCycleTime , *vel_for_convyr, *theta);
                }
                else
                {
                    *theta = trans_from_encoder;
                    zucmotStatus->status_acc_convyr = 2;  // changed according to encoder
                    // rtapi_print("[*vel_for_convyr < last_vel_for_convy] && [not over max_acc, acc = %f]   --->  recalculate *vel_for_convyr = %f, *theta = %f, [---2]\n",
                    // 				fabs(zucmotStatus->last_vel_for_convy - *vel_for_convyr) / zucmotConfig->trajCycleTime , *vel_for_convyr, *theta);
                }
            }
            else
            {
                temp_vel = (trans_from_encoder - zucmotStatus->last_theta) * (1.0 / zucmotConfig->trajCycleTime) * zucmotStatus->r_for_convyr;
                if (fabs(temp_vel - zucmotStatus->last_vel_for_convy) * (1.0 / zucmotConfig->trajCycleTime) <= max_for_acc)
                {
                    *theta = trans_from_encoder;
                    *vel_for_convyr = temp_vel;
                    zucmotStatus->status_acc_convyr = 2;  // cahnged according to encoder
                    // rtapi_print("temp_vel = %f, trans_from_encoder = %f, zucmotStatus->last_theta = %f, [---2]\n", temp_vel, trans_from_encoder, zucmotStatus->last_theta);
                }
            }
        }
        zucmotStatus->first_cycle_in_tracking = 0;
        break;
    case 1:
        /* decelerate */
        // rtapi_print(" -------------------------------- enter [1] -------------------------------- \n");
        *vel_for_convyr = zucmotStatus->last_vel_for_convy - max_for_dec * zucmotConfig->trajCycleTime;  // unit: mm/s
        if (*vel_for_convyr < 0.8 * vel_from_encoder)
        {
            *vel_for_convyr = 0.8 * vel_from_encoder;
        }
        omega = *vel_for_convyr / zucmotStatus->r_for_convyr;
        *theta = zucmotStatus->last_theta + omega * zucmotConfig->trajCycleTime;
        if (*theta < trans_from_encoder)
        {
            *vel_for_convyr = (trans_from_encoder - zucmotStatus->last_theta) * (1.0 / zucmotConfig->trajCycleTime) * zucmotStatus->r_for_convyr;
            // rtapi_print("[*theta < trans_from_encoder] ---> *vel_for_convyr = %f, zucmotStatus->last_vel_for_convy = %f\n", *vel_for_convyr, zucmotStatus->last_vel_for_convy);
            if (*vel_for_convyr > zucmotStatus->last_vel_for_convy)
            {
                if (fabs(zucmotStatus->last_vel_for_convy - *vel_for_convyr) * (1.0 / zucmotConfig->trajCycleTime) > max_for_acc)
                {
                    *vel_for_convyr = zucmotStatus->last_vel_for_convy + max_for_acc * zucmotConfig->trajCycleTime;
                    omega = *vel_for_convyr / zucmotStatus->r_for_convyr;
                    *theta = zucmotStatus->last_theta + omega * zucmotConfig->trajCycleTime;
                    zucmotStatus->status_acc_convyr = 0;  // begin accelerate
                    // rtapi_print("[*vel_for_convyr > last_vel_for_convy] && [over max_acc, acc = %f]   --->  recalculate *vel_for_convyr = %f, *theta = %f, [---0]\n",
                    // 				fabs(zucmotStatus->last_vel_for_convy - *vel_for_convyr) / zucmotConfig->trajCycleTime , *vel_for_convyr, *theta);
                }
                else
                {
                    *theta = trans_from_encoder;
                    zucmotStatus->status_acc_convyr = 2;  // cahnged according to encoder
                    // rtapi_print("[*vel_for_convyr > last_vel_for_convy] && [not over max_acc, acc = %f]   --->  recalculate *vel_for_convyr = %f, *theta = %f, [---2]\n",
                    // 				fabs(zucmotStatus->last_vel_for_convy - *vel_for_convyr) / zucmotConfig->trajCycleTime , *vel_for_convyr, *theta);
                }
            }
            else
            {
                temp_vel = fabs(trans_from_encoder - zucmotStatus->last_theta) * (1.0 / zucmotConfig->trajCycleTime) * zucmotStatus->r_for_convyr;
                if (fabs(temp_vel - zucmotStatus->last_vel_for_convy) * (1.0 / zucmotConfig->trajCycleTime) <= max_for_acc)
                {
                    *theta = trans_from_encoder;
                    *vel_for_convyr = temp_vel;
                    zucmotStatus->status_acc_convyr = 2;  // cahnged according to encoder
                    // rtapi_print("temp_vel = %f, trans_from_encoder = %f, zucmotStatus->last_theta = %f, [---2]\n", temp_vel, trans_from_encoder, zucmotStatus->last_theta);
                }
            }
        }
        else if (*vel_for_convyr < 1e-8 && vel_from_encoder < 1e-8)
        {
            zucmotStatus->status_acc_convyr = 3;
        }
        break;
    case 2:
        /* changed according to encoder */
        // rtapi_print("-------------------------------- enter [2] -------------------------------- \n");
        *theta = trans_from_encoder;
        *vel_for_convyr = (trans_from_encoder - zucmotStatus->last_theta) * (1.0 / zucmotConfig->trajCycleTime) * zucmotStatus->r_for_convyr;
        if (*vel_for_convyr < zucmotStatus->last_vel_for_convy)
        {
            // rtapi_print("[*vel_for_convyr < zucmotStatus->last_vel_for_convy]  ---> \n");
            if ((zucmotStatus->last_vel_for_convy - *vel_for_convyr) * (1.0 / zucmotConfig->trajCycleTime) > max_for_acc)
            {
                *vel_for_convyr = zucmotStatus->last_vel_for_convy - max_for_acc * zucmotConfig->trajCycleTime;
                omega = *vel_for_convyr / zucmotStatus->r_for_convyr;
                *theta = zucmotStatus->last_theta + omega * zucmotConfig->trajCycleTime;
                zucmotStatus->status_acc_convyr = 1;  // begin decelerate
                // rtapi_print("[over max_acc, acc = %f], *vel_for_convyr = %f, zucmotStatus->last_vel_for_convy = %f, *theta = %f, trans_from_encoder = %f\n",
                // 			(zucmotStatus->last_vel_for_convy - *vel_for_convyr) / zucmotConfig->trajCycleTime, *vel_for_convyr, zucmotStatus->last_vel_for_convy, *theta, trans_from_encoder);
            }
            else
            {
                zucmotStatus->status_acc_convyr = 3;
            }
        }
        else if (*vel_for_convyr >= zucmotStatus->last_vel_for_convy)
        {
            // rtapi_print("[*vel_for_convyr > zucmotStatus->last_vel_for_convy]  ---> \n");
            if ((*vel_for_convyr - zucmotStatus->last_vel_for_convy) * (1.0 / zucmotConfig->trajCycleTime) > max_for_acc)
            {
                // rtapi_print("[over max_acc, acc = %f], *vel_for_convyr = %f, zucmotStatus->last_vel_for_convy = %f, *theta = %f, trans_from_encoder = %f\n",
                // 			(zucmotStatus->last_vel_for_convy - *vel_for_convyr) / zucmotConfig->trajCycleTime, *vel_for_convyr, zucmotStatus->last_vel_for_convy, *theta, trans_from_encoder);
                *vel_for_convyr = zucmotStatus->last_vel_for_convy + max_for_acc * zucmotConfig->trajCycleTime;
                omega = *vel_for_convyr / zucmotStatus->r_for_convyr;
                *theta = zucmotStatus->last_theta + omega * zucmotConfig->trajCycleTime;
                zucmotStatus->status_acc_convyr = 0;  // begin acclerate
                                                      // rtapi_print("after, *vel_for_convyr = %f, *theta = %f, trans_from_encoder = %f\n",
                                                      // 			*vel_for_convyr, *theta, trans_from_encoder);
            }
            else
            {
                zucmotStatus->status_acc_convyr = 3;
            }
        }
        break;
    case 3:
        /* changed according to encoder */
        // rtapi_print("-------------------------------- enter [3] -------------------------------- \n");

        if (first_case3)
        {
            rtapi_print("-------------------------------- first enter [3] -------------------------------- \n");
            rtapi_print("trans_from_encoder = %f, zucmotStatus->uhiData.pulses = %ld\n", trans_from_encoder, zucmotStatus->uhiData.pulses);
        }
        *theta = trans_from_encoder;
        // trans_from_encoder = (zucmotStatus->uhiData.pulses - zucmotStatus->conveyorOrigin)*0.00001*zucmotStatus->conveyorPulseEquivalent;
        *vel_for_convyr = (trans_from_encoder - zucmotStatus->last_theta) * (1.0 / zucmotConfig->trajCycleTime) * zucmotStatus->r_for_convyr;
        if ((fabs(*vel_for_convyr) < 1e-4) && (fabs(*vel_for_convyr - zucmotStatus->last_vel_for_convy) * (1.0 / zucmotConfig->trajCycleTime) > max_for_acc))
        {
            *vel_for_convyr = zucmotStatus->last_vel_for_convy;
            // rtapi_print("*vel_for_convyr =  zucmotStatus->last_vel_for_convy = %f %f\n", *vel_for_convyr, zucmotStatus->last_vel_for_convy);
        }
        break;
    default:
        break;
    }
    last_trans_from_encoder = trans_from_encoder;
    // rtapi_print("[get_vel_theta]  --->   trans_from_encoder = %f, theta = %f\n", trans_from_encoder, *theta);
    // rtapi_print("-------------------------------- finished --------------------------------\n");
    return;
}

int track_pre_singular()
{
    if (!zucmotStatus->pre_singular)
    {
        return 0;
    }
    rtapi_print(" ######################### pre_singular: zucmotStatus->still_dec = %d, zucmotStatus->tracking = %d, zucmotStatus->status_acc_convyr = %d\n",
                zucmotStatus->still_dec,
                zucmotStatus->tracking,
                zucmotStatus->status_acc_convyr);
    /* set conveyor tracking cfg */
    zucmotStatus->tracking = 0;
    //    tpManager_tpDisableTrack();
    zucmotStatus->conveyorPulseEquivalent = 0;
    zucmotStatus->status_acc_convyr = 3;
    zucmotStatus->new_cycle = 1;
    /* get real joint position according to joints */
    int jntNum = 0;
    double jointPos[ZUCMOT_MAX_JOINTS] = {0.0}, tar_pos_joint[ZUCMOT_MAX_JOINTS] = {0.0};
    for (jntNum = 0; jntNum < zucmotConfig->numJoints; jntNum++)
    {
        jointPos[jntNum] = joints[jntNum].pos_cmd;
        tar_pos_joint[jntNum] = joints[jntNum].pos_cmd;
    }
    /* get the real carte_pos by kinematic forward */
    KINEMATICS_FORWARD_FLAGS l_fflags = 0;
    KINEMATICS_INVERSE_FLAGS l_iflags = 0;
    kinematicsForward(&zucmotConfig->tool_offset,
                      jointPos,
                      &zucmotStatus->disable_convyr_real_pos,
                      &ctrl_fflags,
                      &ctrl_iflags,
                      &zucmotConfig->base_offset,
                      &zucmotConfig->user_offset);
    //$1(ctrl_fflags);
    //zucmotSet_CtrIFLAGES(ctrl_iflags);
    rtapi_print(" ########################## jointPos = %f %f %f %f %f %f\n", jointPos[0], jointPos[1], jointPos[2], jointPos[3], jointPos[4], jointPos[5]);
    rtapi_print("zucmotStatus->disable_convyr_real_pos = %f %f %f %f %f %f\n",
                zucmotStatus->disable_convyr_real_pos.tran.x,
                zucmotStatus->disable_convyr_real_pos.tran.y,
                zucmotStatus->disable_convyr_real_pos.tran.z,
                zucmotStatus->disable_convyr_real_pos.a,
                zucmotStatus->disable_convyr_real_pos.b,
                zucmotStatus->disable_convyr_real_pos.c);
    int result = 0;
    ZucPose tar_pos;
    tar_pos = zucmotStatus->disable_convyr_real_pos;  //init tar_pos
    // double tar_pos_joint[9];

    if (zucmotStatus->convyr_type == 2)
    {  //若是圆形传送带跟踪，则求减速至零转过的角度并根据旋转变换求速度为零时的笛卡尔和关节位置
        if (zucmotStatus->r_for_convyr != 0.0)
        {
            zucmotStatus->left_theta = pmSq(zucmotStatus->last_vel_for_convy) / 2.0 / zucmotStatus->r_for_convyr / 3500.0;  //zucmotStatus->r_for_convyr==0???,
        }
        else
        {
            zucmotStatus->left_theta = 0.0;
        }
    }
    else
    {
        zucmotStatus->left_theta = pmSq(zucmotStatus->last_vel_for_convy) / 2.0 / 3500.0;  //v^2 = 2*a*s
    }
    get_tar_pos(zucmotStatus->left_theta, &tar_pos);
    result = kinematicsInverse(
        &zucmotConfig->tool_offset, &tar_pos, tar_pos_joint, &ctrl_iflags, &ctrl_fflags, &zucmotConfig->base_offset, &zucmotConfig->user_offset);
    rtapi_print(" ######################## left_theta = %f ########################\n", zucmotStatus->left_theta);
    rtapi_print("tar_pos = %f %f %f %f %f %f\n, tar_pos_joint = %f %f %f %f %f %f\n",
                tar_pos.tran.x,
                tar_pos.tran.y,
                tar_pos.tran.z,
                tar_pos.a,
                tar_pos.b,
                tar_pos.c,
                tar_pos_joint[0],
                tar_pos_joint[1],
                tar_pos_joint[2],
                tar_pos_joint[3],
                tar_pos_joint[4],
                tar_pos_joint[5]);
    if (result != 0 || (zucmotStatus->convyr_type == 2 && zucmotStatus->r_for_convyr < 1e-8))
    {
        // for (jntNum = 0; jntNum < zucmotConfig->numJoints; jntNum++) { zucmotDebug->coord_tp.currentPos_joint[jntNum] = jointPos[jntNum]; }
        // tpSetPos(&zucmotDebug->coord_tp, &zucmotStatus->disable_convyr_real_pos, jointPos);
        // zucmotStatus->carte_pos_desired = zucmotDebug->coord_tp.currentPos;
        //        tpManager_tpSetCurJpos(jointPos);
        //        tpManager_tpSetPos_JPos(&zucmotStatus->disable_convyr_real_pos, jointPos);
        //        tpManager_tpSetPose(zucmotStatus->carte_pos_desired);
        zucmotStatus->still_dec = 0;
    }
    else
    {
        //        tpManager_tpSetPos_JPos(&tar_pos, tar_pos_joint);
        //        tpManager_tpSetPose(zucmotStatus->carte_pos_desired);
        zucmotStatus->still_dec = 1;
    }
    return 0;
}

int max_distance_stop()
{
    if (!zucmotStatus->maxdis_judge)
    {
        return 0;
    }
    else
    {
        zucmotStatus->status_acc_convyr = 3;
        reportError(ERROR_EXCEEDING_THE_MAXIMUM_DISTANCE, "The robot exceeds the maximum distance that the conveyor belt tracks");
    }

    rtapi_print(
        " ######################### max_distance_stop: zucmotStatus->still_dec = %d, zucmotStatus->tracking = %d, zucmotStatus->status_acc_convyr = %d\n",
        zucmotStatus->still_dec,
        zucmotStatus->tracking,
        zucmotStatus->status_acc_convyr);
    /* set conveyor tracking cfg */
    zucmotStatus->tracking = 0;
    //    tpManager_tpDisableTrack();
    zucmotStatus->conveyorPulseEquivalent = 0;
    zucmotStatus->status_acc_convyr = 3;
    zucmotStatus->new_cycle = 1;
    /* get real joint position according to joints */
    int jntNum = 0;
    double jointPos[ZUCMOT_MAX_JOINTS] = {0.0}, tar_pos_joint[ZUCMOT_MAX_JOINTS] = {0.0};
    for (jntNum = 0; jntNum < zucmotConfig->numJoints; jntNum++)
    {
        jointPos[jntNum] = joints[jntNum].pos_cmd;
        tar_pos_joint[jntNum] = joints[jntNum].pos_cmd;
    }
    /* get the real carte_pos by kinematic forward */
    KINEMATICS_FORWARD_FLAGS l_fflags = 0;
    KINEMATICS_INVERSE_FLAGS l_iflags = 0;
    kinematicsForward(&zucmotConfig->tool_offset,
                      jointPos,
                      &zucmotStatus->disable_convyr_real_pos,
                      &ctrl_fflags,
                      &ctrl_iflags,
                      &zucmotConfig->base_offset,
                      &zucmotConfig->user_offset);
    //$1(ctrl_fflags);
    //zucmotSet_CtrIFLAGES(ctrl_iflags);
    rtapi_print(" ########################## max_distance_stop---jointPos = %f %f %f %f %f %f\n",
                jointPos[0],
                jointPos[1],
                jointPos[2],
                jointPos[3],
                jointPos[4],
                jointPos[5]);
    rtapi_print("zucmotStatus->disable_convyr_real_pos = %f %f %f %f %f %f\n",
                zucmotStatus->disable_convyr_real_pos.tran.x,
                zucmotStatus->disable_convyr_real_pos.tran.y,
                zucmotStatus->disable_convyr_real_pos.tran.z,
                zucmotStatus->disable_convyr_real_pos.a,
                zucmotStatus->disable_convyr_real_pos.b,
                zucmotStatus->disable_convyr_real_pos.c);
    int result = 0;
    ZucPose tar_pos;
    tar_pos = zucmotStatus->disable_convyr_real_pos;  //init tar_pos
    // double tar_pos_joint[9];

    if (zucmotStatus->convyr_type == 2)
    {  //若是圆形传送带跟踪，则求减速至零转过的角度并根据旋转变换求速度为零时的笛卡尔和关节位置
        if (zucmotStatus->r_for_convyr != 0.0)
        {
            zucmotStatus->left_theta = pmSq(zucmotStatus->last_vel_for_convy) / 2.0 / zucmotStatus->r_for_convyr / 3500.0;  //zucmotStatus->r_for_convyr==0???,
        }
        else
        {
            zucmotStatus->left_theta = 0.0;
        }
    }
    else
    {
        zucmotStatus->left_theta = pmSq(zucmotStatus->last_vel_for_convy) / 2.0 / 3500.0;  //v^2 = 2*a*s
    }
    get_tar_pos(zucmotStatus->left_theta, &tar_pos);
    result = kinematicsInverse(
        &zucmotConfig->tool_offset, &tar_pos, tar_pos_joint, &ctrl_iflags, &ctrl_fflags, &zucmotConfig->base_offset, &zucmotConfig->user_offset);
    rtapi_print(" ######################## max_distance_stop---left_theta = %f ########################\n", zucmotStatus->left_theta);
    rtapi_print("tar_pos = %f %f %f %f %f %f\n, tar_pos_joint = %f %f %f %f %f %f\n",
                tar_pos.tran.x,
                tar_pos.tran.y,
                tar_pos.tran.z,
                tar_pos.a,
                tar_pos.b,
                tar_pos.c,
                tar_pos_joint[0],
                tar_pos_joint[1],
                tar_pos_joint[2],
                tar_pos_joint[3],
                tar_pos_joint[4],
                tar_pos_joint[5]);
    if (result != 0 || (zucmotStatus->convyr_type == 2 && zucmotStatus->r_for_convyr < 1e-8))
    {
        // for (jntNum = 0; jntNum < zucmotConfig->numJoints; jntNum++) { zucmotDebug->coord_tp.currentPos_joint[jntNum] = jointPos[jntNum]; }
        // tpSetPos(&zucmotDebug->coord_tp, &zucmotStatus->disable_convyr_real_pos, jointPos);
        // zucmotStatus->carte_pos_desired = zucmotDebug->coord_tp.currentPos;
        //        tpManager_tpSetCurJpos(jointPos);
        //        tpManager_tpSetPos_JPos(&zucmotStatus->disable_convyr_real_pos, jointPos);
        //        tpManager_tpSetPose(zucmotStatus->carte_pos_desired);
        zucmotStatus->still_dec = 0;
    }
    else
    {
        //        tpManager_tpSetPos_JPos(&tar_pos, tar_pos_joint);
        //        tpManager_tpSetPose(zucmotStatus->carte_pos_desired);
        zucmotStatus->still_dec = 1;
    }
    return 0;
}

ZucPose getFinalUserOffset()
{
    ZucPose finalUserOffset = zucmotConfig->user_offset;
    //double trans_from_encoder = 0.0;                               //根据编码器计算得到的应该进行的变换
    double vel_for_convyr = 0.0, theta = 0.0, max_for_dec = zucmotConfig->synch_acc;  //为复合传送带运动的速度,加速度,半径,角速度
    if (zucmotStatus->pre_singular || zucmotStatus->maxdis_judge)
    {
        max_for_dec = 3500.0;  //传送带跟踪过程中预判奇异的时候，使用3500mm/s^2的减速度进行减速
    }
    if (zucmotStatus->tracking || (zucmotStatus->still_dec == 1 && zucmotStatus->tracking == 0))
    {
        double tracking_length = 0.0;
        if (zucmotStatus->convyr_type == 1)
        {
            if (zucmotStatus->still_dec == 1 && zucmotStatus->tracking == 0)
            {
                //正处于退出传送带后的减速阶段
                if (zucmotStatus->new_cycle)
                {  // add new_cycle for not go into getFinalUserOffset twice
                    vel_for_convyr = zucmotStatus->last_vel_for_convy - max_for_dec * zucmotConfig->trajCycleTime;
                    if (vel_for_convyr < 0.0)
                    {
                        vel_for_convyr = 0.0;
                    }
                    theta = (0.5 * (vel_for_convyr + zucmotStatus->last_vel_for_convy)) * zucmotConfig->trajCycleTime;
                    if (theta > zucmotStatus->left_theta)
                    {
                        theta = zucmotStatus->left_theta;
                    }
                    zucmotStatus->left_theta = zucmotStatus->left_theta - theta;
                    zucmotStatus->last_vel_for_convy = vel_for_convyr;
                    if (zucmotStatus->left_theta < DOUBLE_FUZZ || vel_for_convyr < DOUBLE_FUZZ)
                    {
                        zucmotStatus->still_dec = 0;
                    }
                    rtapi_print(" [getFinalUserOffet]******************************************************************************************** last_theta = "
                                "%f, left_theta = %f, theta = %f\n",
                                zucmotStatus->last_theta,
                                zucmotStatus->left_theta,
                                theta);
                    tracking_length = zucmotStatus->last_theta + theta;

                    zucmotStatus->new_cycle = 0;
                    zucmotStatus->last_theta = zucmotStatus->last_theta + theta;
                }
            }
            else
            {
                get_vel_theta(&vel_for_convyr, &theta);
                if (fabs(theta - zucmotStatus->last_theta) > 1e-4)
                {
                    zucmotStatus->last_vel_for_convy = vel_for_convyr;
                }
                zucmotStatus->last_theta = theta;
                tracking_length = theta;  //考虑速度突变
            }
            finalUserOffset.tran.x += zucmotStatus->conveyorDir.x * tracking_length;
            finalUserOffset.tran.y += zucmotStatus->conveyorDir.y * tracking_length;
            finalUserOffset.tran.z += zucmotStatus->conveyorDir.z * tracking_length;
        }
        else if (zucmotStatus->convyr_type == 2)
        {
            PmRpy rpy_desired, delta_rpy, usr_to_world_rpy, final_rpy;
            PmRotationMatrix rot_desired, delta_rot, usr_to_world_rot;
            PmHomogeneous desired_to_usr_t, desired_to_o_t, delta_t_to_o, usr_to_o_t, usr_to_world_t, world_to_o_t, o_to_usr_t, temp_t1, temp_t2,
                final_usr_to_world_t;
            //获得当前TCP相对于当前用户坐标系的齐次矩阵,用以计算r_for_convyr
            rpy_desired.r = zucmotStatus->carte_pos_desired.c * PM_PI / 180.0;
            rpy_desired.p = zucmotStatus->carte_pos_desired.b * PM_PI / 180.0;
            rpy_desired.y = zucmotStatus->carte_pos_desired.a * PM_PI / 180.0;
            pmRpyMatConvert(&rpy_desired, &rot_desired);
            desired_to_usr_t.rot = rot_desired;
            desired_to_usr_t.tran = zucmotStatus->carte_pos_desired.tran;
            //获得当前用户坐标系相对于传送带旋转圆心坐标系{O}的变换：T{user to O} = T{world to O}*T{user to world}
            usr_to_world_rpy.r = zucmotConfig->user_offset.c * PM_PI / 180.0;
            usr_to_world_rpy.p = zucmotConfig->user_offset.b * PM_PI / 180.0;
            usr_to_world_rpy.y = zucmotConfig->user_offset.a * PM_PI / 180.0;
            pmRpyMatConvert(&usr_to_world_rpy, &usr_to_world_rot);
            usr_to_world_t.rot = usr_to_world_rot;
            usr_to_world_t.tran = zucmotConfig->user_offset.tran;
            pmHomInv(&zucmotStatus->o_to_world, &world_to_o_t);
            pmHomHomMult(&world_to_o_t, &usr_to_world_t, &usr_to_o_t);
            //获得传送带旋转圆心坐标系{O}相对于当前用户坐标系的变换
            pmHomInv(&usr_to_o_t, &o_to_usr_t);
            //获得当前TCP相对于传送带圆心坐标系的齐次矩阵,用以计算r_for_convyr
            pmHomHomMult(&usr_to_o_t, &desired_to_usr_t, &desired_to_o_t);
            // rtapi_print("[getFinalUserOffsets] ---> zucmotStatus->still_dec = %d, zucmotStatus->tracking = %d\n", zucmotStatus->still_dec, zucmotStatus->tracking);
            if (zucmotStatus->still_dec == 1 && zucmotStatus->tracking == 0)
            {  //正处于退出传送带后的减速阶段
                if (zucmotStatus->new_cycle)
                {  // add new_cycle for not go into getFinalUserOffset twice
                    vel_for_convyr = zucmotStatus->last_vel_for_convy - max_for_dec * zucmotConfig->trajCycleTime;
                    if (vel_for_convyr < 0.0)
                    {
                        vel_for_convyr = 0.0;
                    }
                    theta = (0.5 * (vel_for_convyr + zucmotStatus->last_vel_for_convy)) / zucmotStatus->r_for_convyr * zucmotConfig->trajCycleTime;
                    if (theta > zucmotStatus->left_theta)
                    {
                        theta = zucmotStatus->left_theta;
                    }
                    zucmotStatus->left_theta = zucmotStatus->left_theta - theta;
                    zucmotStatus->last_vel_for_convy = vel_for_convyr;
                    if (zucmotStatus->left_theta < DOUBLE_FUZZ || vel_for_convyr < DOUBLE_FUZZ)
                    {
                        zucmotStatus->still_dec = 0;
                    }
                    rtapi_print(" [getFinalUserOffet]******************************************************************************************** last_theta = "
                                "%f, left_theta = %f, theta = %f\n",
                                zucmotStatus->last_theta,
                                zucmotStatus->left_theta,
                                theta);
                    delta_rpy.r = zucmotStatus->last_theta + theta;
                    zucmotStatus->new_cycle = 0;
                    // rtapi_print("[1] before zucmotStatus->last_theta = %f\n", zucmotStatus->last_theta);
                    zucmotStatus->last_theta = zucmotStatus->last_theta + theta;
                    // rtapi_print("[1] after zucmotStatus->last_theta = %f\n", zucmotStatus->last_theta);
                }

                // delta_rpy.r = zucmotStatus->last_theta + theta;
                // zucmotStatus->new_cycle = 0;
                // rtapi_print("[1] before zucmotStatus->last_theta = %f\n", zucmotStatus->last_theta);
                // zucmotStatus->last_theta = zucmotStatus->last_theta + theta;
                // rtapi_print("[1] after zucmotStatus->last_theta = %f\n", zucmotStatus->last_theta);
            }
            else
            {
                zucmotStatus->r_for_convyr = pmSqrt(pmSq(desired_to_o_t.tran.x) + pmSq(desired_to_o_t.tran.y));  // unit: mm
                get_vel_theta(&vel_for_convyr, &theta);
                if (fabs(theta - zucmotStatus->last_theta) > 1e-4)
                {
                    zucmotStatus->last_vel_for_convy = vel_for_convyr;
                }
                // rtapi_print("[2] before zucmotStatus->last_theta = %f\n", zucmotStatus->last_theta);
                zucmotStatus->last_theta = theta;
                // rtapi_print("[2] after zucmotStatus->last_theta = %f\n", zucmotStatus->last_theta);
                // rtapi_print("zucmotStatus->status_acc_cony = %d, zucmotStatus->last_vel_for_convy = %f, zucmotStatus->last_theta = %f\n", zucmotStatus->status_acc_convyr,
                // 			zucmotStatus->last_vel_for_convy, zucmotStatus->last_theta);
                // rtapi_print("______________[getFinalUserOffsets]________________ zucmotStatus->uhiData.pulses = %ld\n", zucmotStatus->uhiData.pulses);
                //获得相对于{O}的变换，比较简单，只是绕z轴的旋转
                delta_rpy.r = theta;  //考虑速度突变
                                      // delta_rpy.r = (zucmotStatus->uhiData.pulses - zucmotStatus->conveyorOrigin)*zucmotStatus->conveyorPulseEquivalent;;
            }

            delta_rpy.p = 0.0;
            delta_rpy.y = 0.0;
            pmRpyMatConvert(&delta_rpy, &delta_rot);
            delta_t_to_o.rot = delta_rot;
            delta_t_to_o.tran.x = 0.0;
            delta_t_to_o.tran.y = 0.0;
            delta_t_to_o.tran.z = 0.0;

            //获得当前用户坐标系需右乘的变换矩阵
            pmHomHomMult(&o_to_usr_t, &delta_t_to_o, &temp_t1);
            pmHomHomMult(&temp_t1, &usr_to_o_t, &temp_t2);
            pmHomHomMult(&usr_to_world_t, &temp_t2, &final_usr_to_world_t);
            //将final_offset表示为x,y,z,a,b,c的形式
            pmMatRpyConvert(&(final_usr_to_world_t.rot), &final_rpy);
            finalUserOffset.a = final_rpy.y / PM_PI * 180.0;
            finalUserOffset.b = final_rpy.p / PM_PI * 180.0;
            finalUserOffset.c = final_rpy.r / PM_PI * 180.0;
            finalUserOffset.tran = final_usr_to_world_t.tran;
        }
        if ((fabs((zucmotStatus->uhiData.pulses - zucmotStatus->conveyorOrigin) * zucmotStatus->conveyorPulseEquivalent) >=
             zucmotStatus->conveyorMaxDistance) &&
            zucmotStatus->ValidDistance)
        {
            zucmotStatus->maxdis_judge = 1;
            rtapi_print(" max_distance_stop during conveyor tracking\n");
            SET_MOTION_ERROR_FLAG(1);
            //            tpManager_tpabort();
            motAbortMotion();
            // reportError(ERROR_EXCEEDING_THE_MAXIMUM_DISTANCE, "The robot exceeds the maximum distance that the conveyor belt tracks");
        }
    }
    return finalUserOffset;
}

void print_pos_msg()
{
    rtapi_print(">>>Failed cartesian pos: %f, %f, %f, %f, %f, %f\n",
                zucmotStatus->carte_pos_cmd.tran.x,
                zucmotStatus->carte_pos_cmd.tran.y,
                zucmotStatus->carte_pos_cmd.tran.z,
                zucmotStatus->carte_pos_cmd.a,
                zucmotStatus->carte_pos_cmd.b,
                zucmotStatus->carte_pos_cmd.c);
    double currentPos_joint[ZUCMOT_MAX_JOINTS] = {};
    //    tpManager_tpGetCurJointPos(currentPos_joint);
    rtapi_print(">>> current joint pos: %f, %f, %f, %f, %f, %f\n",
                currentPos_joint[0],
                currentPos_joint[1],
                currentPos_joint[2],
                currentPos_joint[3],
                currentPos_joint[4],
                currentPos_joint[5]);
    rtapi_print(">>> current user_offset when failed cartesian pos: %f, %f, %f, %f, %f, %f\n",
                zucmotConfig->user_offset.tran.x,
                zucmotConfig->user_offset.tran.y,
                zucmotConfig->user_offset.tran.z,
                zucmotConfig->user_offset.a,
                zucmotConfig->user_offset.b,
                zucmotConfig->user_offset.c);
    rtapi_print(">>> current tool_offset when failed cartesian pos: %f, %f, %f, %f, %f, %f\n",
                zucmotConfig->tool_offset.tran.x,
                zucmotConfig->tool_offset.tran.y,
                zucmotConfig->tool_offset.tran.z,
                zucmotConfig->tool_offset.a,
                zucmotConfig->tool_offset.b,
                zucmotConfig->tool_offset.c);
    rtapi_print(">>> current base_offset when failed cartesian pos: %f, %f, %f\n",
                zucmotConfig->base_offset.y,
                zucmotConfig->base_offset.p,
                zucmotConfig->base_offset.r);
    if (zucmotStatus->tracking)
    {
        rtapi_print(">>> during conveyor tracking: turns_count = %d, convayorCnt = %ld  conveyorOrigin = %ld \n",
                    zucmotStatus->turns_count,
                    zucmotStatus->uhiData.pulses,
                    zucmotStatus->conveyorOrigin);
    }
}

ZucPose tracking_get_revisedpos()  //完成圆形传送带跟踪功能的另一种思路,对目标位置进行坐标变换;目前没有用该思路
{
    ZucPose revised_pos;  // after considering conveyor belt
    PmRpy rpy_desired, delta_rpy, revised_rpy;
    PmRotationMatrix rot_desired, delta_rot;
    PmHomogeneous t_desired, t_usroffset_to_o, delta_t_to_o, t_revised1, t_revised2, t_revised;

    rpy_desired.r = zucmotStatus->carte_pos_desired.c * PM_PI / 180.0;
    rpy_desired.p = zucmotStatus->carte_pos_desired.b * PM_PI / 180.0;
    rpy_desired.y = zucmotStatus->carte_pos_desired.a * PM_PI / 180.0;
    pmRpyMatConvert(&rpy_desired, &rot_desired);
    // rtapi_print("rpy_desired = %f %f %f\n", rpy_desired.r, rpy_desired.p, rpy_desired.y);
    // rtapi_print("[tracking_get_revisedpos]  ---> rot_desired = \n");
    // rtapi_print("%f %f %f\n", rot_desired.x.x, rot_desired.y.x, rot_desired.z.x);
    // rtapi_print("%f %f %f\n", rot_desired.x.y, rot_desired.y.y, rot_desired.z.y);
    // rtapi_print("%f %f %f\n", rot_desired.x.z, rot_desired.y.z, rot_desired.z.z);

    t_desired.rot = rot_desired;
    t_desired.tran = zucmotStatus->carte_pos_desired.tran;

    pmHomInv(&zucmotStatus->o_to_world, &t_usroffset_to_o);

    delta_rpy.r = (zucmotStatus->uhiData.pulses - zucmotStatus->conveyorOrigin) * zucmotStatus->conveyorPulseEquivalent;  //绕{O}的z轴转过的弧度, rad
    delta_rpy.p = 0.0;
    delta_rpy.y = 0.0;
    pmRpyMatConvert(&delta_rpy, &delta_rot);
    // rtapi_print("pulses = %f, conveyorOrigin = %f, conveyorPulseEquivalent = %f\n", zucmotStatus->uhiData.pulses, zucmotStatus->conveyorOrigin, zucmotStatus->conveyorPulseEquivalent);
    // rtapi_print("[tracking_get_revisedpos]  ---> delta_rot = \n");
    // rtapi_print("%f %f %f\n", delta_rot.x.x, delta_rot.y.x, delta_rot.z.x);
    // rtapi_print("%f %f %f\n", delta_rot.x.y, delta_rot.y.y, delta_rot.z.y);
    // rtapi_print("%f %f %f\n", delta_rot.x.z, delta_rot.y.z, delta_rot.z.z);

    // rtapi_print("[tracking_get_revisedpos]  ---> o_to_world.rot = \n");
    // rtapi_print("%f %f %f\n", zucmotStatus->o_to_world.rot.x.x, zucmotStatus->o_to_world.rot.y.x, zucmotStatus->o_to_world.rot.z.x);
    // rtapi_print("%f %f %f\n", zucmotStatus->o_to_world.rot.x.y, zucmotStatus->o_to_world.rot.y.y, zucmotStatus->o_to_world.rot.z.y);
    // rtapi_print("%f %f %f\n", zucmotStatus->o_to_world.rot.x.z, zucmotStatus->o_to_world.rot.y.z, zucmotStatus->o_to_world.rot.z.z);

    // rtapi_print("[tracking_get_revisedpos]  ---> t_usroffset_to_o.rot = \n");
    // rtapi_print("%f %f %f\n", t_usroffset_to_o.rot.x.x, t_usroffset_to_o.rot.y.x, t_usroffset_to_o.rot.z.x);
    // rtapi_print("%f %f %f\n", t_usroffset_to_o.rot.x.y, t_usroffset_to_o.rot.y.y, t_usroffset_to_o.rot.z.y);
    // rtapi_print("%f %f %f\n", t_usroffset_to_o.rot.x.z, t_usroffset_to_o.rot.y.z, t_usroffset_to_o.rot.z.z);
    delta_t_to_o.rot = delta_rot;
    delta_t_to_o.tran.x = 0.0;
    delta_t_to_o.tran.y = 0.0;
    delta_t_to_o.tran.z = 0.0;
    pmHomHomMult(&zucmotStatus->o_to_world, &delta_t_to_o, &t_revised1);
    // rtapi_print("[tracking_get_revisedpos]  ---> t_revised1.rot = \n");
    // rtapi_print("%f %f %f\n", t_revised1.rot.x.x, t_revised1.rot.y.x, t_revised1.rot.z.x);
    // rtapi_print("%f %f %f\n", t_revised1.rot.x.y, t_revised1.rot.y.y, t_revised1.rot.z.y);
    // rtapi_print("%f %f %f\n", t_revised1.rot.x.z, t_revised1.rot.y.z, t_revised1.rot.z.z);

    pmHomHomMult(&t_revised1, &t_usroffset_to_o, &t_revised2);

    pmHomHomMult(&t_revised2, &t_desired, &t_revised);
    pmMatRpyConvert(&(t_revised.rot), &revised_rpy);
    // rtapi_print("revised_rpy = %f %f %f\n", revised_rpy.r, revised_rpy.p, revised_rpy.y);
    // rtapi_print("[tracking_get_revisedpos]  ---> t_revised.rot = \n");
    // rtapi_print("%f %f %f\n", t_revised.rot.x.x, t_revised.rot.y.x, t_revised.rot.z.x);
    // rtapi_print("%f %f %f\n", t_revised.rot.x.y, t_revised.rot.y.y, t_revised.rot.z.y);
    // rtapi_print("%f %f %f\n", t_revised.rot.x.z, t_revised.rot.y.z, t_revised.rot.z.z);

    revised_pos.tran = t_revised.tran;
    revised_pos.a = revised_rpy.y / PM_PI * 180.0;
    revised_pos.b = revised_rpy.p / PM_PI * 180.0;
    revised_pos.c = revised_rpy.r / PM_PI * 180.0;
    // rtapi_print("desired.tran = %f %f %f\n", zucmotStatus->carte_pos_desired.tran.x, zucmotStatus->carte_pos_desired.tran.y, zucmotStatus->carte_pos_desired.tran.z);
    // rtapi_print("revised_pos.tran = %f %f %f\n", revised_pos.tran.x, revised_pos.tran.y, revised_pos.tran.z);
    return revised_pos;
}

static int handleCoordError()
{
    if (!bHitSingularity)
    {
        // int joint_num = 0;
        bHitSingularity = 1;
        print_pos_msg();
        if ((zucmot_hal_data->motion_error_code) != STOP_ON_SIGULARITY || !GET_MOTION_ERROR_FLAG())
            reportError(STOP_ON_SIGULARITY, _("Protective stop since it's nearby singularity position____1/handleCoordError."));
        SET_MOTION_ERROR_FLAG(1);
        SET_MOTION_INPOS_FLAG(1);       // for mode switching
                                        //        tpManager_tpabort();
        zucmotDebug->coordinating = 0;  // exiting coord mode will reset bHitSingularity flag
        // zucmotDebug->servomoveoperating = 0;
        zucmotDebug->teleoperating = 0;
        zucmotDebug->teleoperating_tool = 0;
        zucmotStatus->compliantType = 0;
        zucmotStatus->disableForceCtrl = 0;

        // for(joint_num=0; joint_num<zucmotConfig->numJoints; joint_num++) {
        // 	joints[joint_num].free_tp.max_acc = 720.0;
        // 	joints[joint_num].free_tp.curr_vel = joints[joint_num].vel_cmd;
        // 	if(joints[joint_num].free_tp.curr_vel > 180) joints[joint_num].free_tp.curr_vel = 180.0;
        // 	if(joints[joint_num].free_tp.curr_vel < -180) joints[joint_num].free_tp.curr_vel = -180.0;
        // }
    }
    return 0;
}

/***********************************************************************
*                        PUBLIC FUNCTION CODE                          *
************************************************************************/

/*
  zucmotController() runs the trajectory and interpolation calculations
  each control cycle

  This function gets called at regular intervals - therefore it does NOT
  have a loop within it!

  Inactive axes are still calculated, but the PIDs are inhibited and
  the amp enable/disable are inhibited
  */
int clear_track(int* first_case3)
{
    zucmotStatus->tracking = 0;
    zucmotStatus->convyr_type = 0;
    zucmotStatus->pre_singular = 0;
    zucmotStatus->maxdis_judge = 0;
    zucmotStatus->still_dec = 0;
    zucmotStatus->new_cycle = 0;
    zucmotStatus->get_conveyor_ori = 0;
    *first_case3 = 1;
    return 0;
}

int checkOutOfRange()
{
    int joint_num = 0;
    zucmot_joint_t* joint;
    for (joint_num = 0; joint_num < zucmotConfig->numJoints; joint_num++)
    {
        joint = &joints[joint_num];
        if (joint->pos_fb >= joint->max_pos_limit || joint->pos_fb <= joint->min_pos_limit)
        {
            rtapi_print(" joint %d out of range pos_fb =%f  max_pos_limit=%f min_pos_limit=%f \n",
                        joint_num,
                        joint->pos_fb,
                        joint->max_pos_limit,
                        joint->min_pos_limit);
            return 1;
        }
    }
    return 0;
}

static int synTIOConfigsForHwV3()
{
    int i = 0;
    ModRtuComm rtuComm;

    // tio vout settings
    tio_set_vout_enable(zucmotConfig->tioConfig.voutEna);
    tio_set_vout_voltage(zucmotConfig->tioConfig.voutVol);

    // rs485 communication config
    rtuComm = zucmotConfig->tioConfig.rsChnCfg[0].rtuComm;
    set_tio_rs485_chn_comm(0, rtuComm.baudrate, rtuComm.databits, rtuComm.stopbits, rtuComm.parity);
    rtuComm = zucmotConfig->tioConfig.rsChnCfg[1].rtuComm;
    set_tio_rs485_chn_comm(1, rtuComm.baudrate, rtuComm.databits, rtuComm.stopbits, rtuComm.parity);

    // channel mode configuration
    for (i = 0; i < 2; i++)
    {
        if (zucmotConfig->tioConfig.rsChnCfg[i].mode == 0)
        {  // modbus RTU
            set_tio_rs485_chn_mode(i, 0);
        }
        else if (zucmotConfig->tioConfig.rsChnCfg[i].mode == 1)
        {  // raw RS485
            set_tio_rs485_chn_mode(i, 1);
        }
        else if (zucmotConfig->tioConfig.rsChnCfg[i].mode == 2)
        {
            set_tio_rs485_chn_mode(i, 2);
        }
    }

    // torque sensor choice
    if (zucmotConfig->tioConfig.rsChnCfg[0].mode == 2)
    {
        tio_set_rs485_chn_func(1);
    }
    else if (zucmotConfig->tioConfig.rsChnCfg[1].mode == 2)
    {
        tio_set_rs485_chn_func(2);
    }
    else
    {
        tio_set_rs485_chn_func(0);
    }

    // pin configuration
    tio_setup_di_pin(zucmotConfig->tioConfig.pinCfg.diCfg);
    tio_setup_do_pin(zucmotConfig->tioConfig.pinCfg.doCfg);
    tio_setup_ai_pin(zucmotConfig->tioConfig.pinCfg.aiCfg);
    return 0;
}

void zucmotController(void* arg)
{
    canmonitor_record_timer(0);
    for (size_t i = 0; i < axisgroup::AXISGROUP_MAX_CNT; i++)
    {
        auto grp = axisgroup::AxisGroupMgr::instance().get_axisgroup(i);
        if (grp)
        {
            grp->update_fsm();
        }
    }

    process_inputs();
    handle_drag_req();
    handle_point_button_req();
    get_pos_cmds();

    //检查是否安全function异常
    // safety_func_check();

    get_curr_cmds();
    handle_servo_error();
    output_to_hal();
    update_status();
    update_servo_enable_delay();

    zucmotStatus->heartbeat++;
}

/***********************************************************************
*                         LOCAL FUNCTION CODE                          *
************************************************************************/
static uint8_t isRobotInBackdriveState()
{
    for (int i = 0; i < zucmotConfig->numJoints; i++)
    {
        if (zucmotDebug->joints[i].backdrive_state)
        {
            return 1;
        }
    }
    return 0;
}

static int poweroff_cnts = 0;
static int poweroff_delay = 0;
static int last_identifying_status = 0;

static void estop_reaction(const int& estop)
{
    if (estop)  // 急停触发
    {
        if ((zucmot_hal_data->zuc_enable_cmd) == 1)
        {
            if (!poweroff_delay && ((zucmot_hal_data->motors_servo_on) != 0))
            {
                (zucmot_hal_data->motors_servo_on) = 0;
                poweroff_delay = 1;
            }
            if (poweroff_delay)
            {
                /*wait 1.2 seconds to trigger zuc_enable_cmd*/
                ++poweroff_cnts;
                if (poweroff_cnts == POWER_OFF_DELAY)
                {
                    (zucmot_hal_data->zuc_enable_cmd) = 0;
                    poweroff_delay = 0;
                    poweroff_cnts = 0;
                }
            }
            else
            {
                /*trigger the estop immediately*/
                (zucmot_hal_data->zuc_enable_cmd) = 0;
            }
        }
        else if ((zucmot_hal_data->motors_servo_on) != 0)
        {
            (zucmot_hal_data->motors_servo_on) = 0;
        }
    }
    else if (poweroff_delay && (zucmot_hal_data->zuc_enable_cmd) == 1)
    {
        (zucmot_hal_data->zuc_enable_cmd) = 0;
        poweroff_delay = 0;
        poweroff_cnts = 0;
    }
}
static void update_axis_fsm()
{
    // //急停则关闭伺服电源请求,等同shutdown
    // if (!zucmotStatus->emergency_stop && (zucmot_hal_data->zuc_enable_cmd))
    // {
    //     zucmot_hal_data->zuc_enable_in = 1;
    // }
    // else
    // {
    //     zucmot_hal_data->zuc_enable_in = 0;
    // }

    // /**
    //  * powered_on = 0以后，taskmain会根据这个状态来发disable的指令
    //  * disable过程中会给伺服发送一系列状态字
    //  * 伺服应该是根据收到的某个状态字来确认不在跟踪控制器给定的
    //  * 所以在下电时候还是恢复原本逻辑，收到下电指令就切换状态
    // */
    // if ((zucmot_hal_data->zuc_enable_in) == 0)  //状态还未上电
    // {
    //     zucmotStatus->powered_on = (zucmot_hal_data->zuc_enable_in) ? 1 : 0;
    //     zucmot_hal_data->robot_powered_on = zucmot_hal_data->zuc_enable_in;
    //     wait_robot_power_on = 0;
    // }

    // if ((zucmot_hal_data->zuc_enable_in) != 0 && get_power_status() == 0 && poweroff_delay == 0)
    // {
    //     //处理上电指令

    //     // zucmotStatus->is_powering_on = 1;  // 开始执行上电操作，set正在上电标记
    //     reportInfo(INFO_ROBOT_POWERED_ON, "robot powered on");
    //     // canmonitor_clear();
    //     // rtbus_motion_bus_active(true);
    //     // init_servo();
    //     // reset_tio();
    //     // set_power_status(1, 0);
    //     if (RUN_MODE_REAL == zucmotConfig->safetyCfg.motRunMode || RUN_MODE_REAL_NO_CAN_CHECK == zucmotConfig->safetyCfg.motRunMode)
    //     {
    //         scb_power_robot(true);

    //         do_robot_query_setting_when_active();

    //         for (int i = 0; i < ZUCMOT_MAX_JOINTS; i++) { queryServoVersionTimeOut[i] = 1; }

    //         for (joint_num = 0; joint_num < zucmotConfig->numJoints; joint_num++) { sync_cab_type(joint_num, zucmotConfig->cab_type); }
    //         wait_robot_power_on = 1;
    //     }
    //     else
    //     {
    //         // zucmotStatus->is_powering_on = 0;  // 仿真模式下，直接上电，不用等待。
    //         zucmotStatus->powered_on = (zucmot_hal_data->zuc_enable_in) ? 1 : 0;
    //         (zucmot_hal_data->robot_powered_on) = (zucmot_hal_data->zuc_enable_in);
    //     }
    //     // printf("\n>>>>>>>>>>>>>>>>>>>>>>>>>joints:  eStop_powerOn\n");
    //     // for (int i = 0; i < zucmotConfig->numJoints; i++) { printf("%lf / %lf \n", joints[i].pos_fb, joints[i].pos_cmd); }
    //     // printf("\n>>>>>>>>>>>>>>>>>>>>>>>>>################################# \n\n");
    // }
    // else if ((zucmot_hal_data->zuc_enable_in) == 0 && get_power_status() == 1 && poweroff_delay == 0)
    // {
    //     //处理上电指令，实际状态已经下电

    //     int bTxOffDelay = GET_MOTION_ENABLE_FLAG() ? 1 : 0;
    //     reportInfo(INFO_ROBOT_POWERED_OFF, "robot powered off");
    //     servoEnable(0);  // servo off first
    //     zucmotDebug->enabling = 0;
    //     if (zucmotStatus->in_homing_procedure)
    //     {
    //         zucmotStatus->in_homing_procedure = 0;  // 下电时清空该状态
    //     }
    //     scb_power_robot(false);
    //     rtbus_motion_bus_active(false);
    //     set_power_status(0, bTxOffDelay);
    //     if (zucmotStatus->category_one_stop_program_pause == 0)
    //     {
    //         if (GET_MOTION_COORD_FLAG())
    //         {
    //             printf(">>>>>>>>>>>>>>>>>>>>>>>> tpAbort\n");
    //    //             tpManager_tpabort();
    //         }
    //         zucmotDebug->coordinating = 0;
    //     }
    //     // printf(">>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>%s[%d]   zucmotDebug->coordinating = 0 \n", __FILE__, __LINE__);
    //     zucmotDebug->teleoperating = 0;
    //     zucmotDebug->teleoperating_tool = 0;
    //     zucmotDebug->dragging = 0;
    //     zucmotDebug->servomoveoperating = 0;
    //     zucmotDebug->admitting = 0;
    //     if (zucmotStatus->protective_stop)
    //     {
    //         zucmotStatus->protective_stop = 0;
    //     }
    // }
}

static void update_axisgroup_fsm()
{
    for (size_t i = 0; i < axisgroup::AXISGROUP_MAX_CNT; i++)
    {
        auto ag = axisgroup::AxisGroupMgr::instance().get_axisgroup(i);
        if (!ag)
        {
            break;
        }
        ag->update_fsm();
    }
    return;

    // zucmotStatus->backdrive_state = isRobotInBackdriveState();
}
static void update_identifying_status()
{
    // if (zucmotStatus->identifying_status == 1 && last_identifying_status != 1)
    // {
    //     if (zucmotStatus->identifying_trajid == 0)
    //     {
    //         if (zucmotStatus->identifying_withpayload == 1)
    //         {
    //             payload_identify_file = fopen("/tmp/traj1withload.txt", "w+");
    //         }
    //         else
    //         {
    //             payload_identify_file = fopen("/tmp/traj1withoutload.txt", "w+");
    //         }
    //     }
    //     else if (zucmotStatus->identifying_trajid == 1)
    //     {
    //         if (zucmotStatus->identifying_withpayload == 1)
    //         {
    //             payload_identify_file = fopen("/tmp/traj2withload.txt", "w+");
    //         }
    //         else
    //         {
    //             payload_identify_file = fopen("/tmp/traj2withoutload.txt", "w+");
    //         }
    //     }
    //     else if (zucmotStatus->identifying_trajid == 2)
    //     {
    //         if (zucmotStatus->identifying_withpayload == 1)
    //         {
    //             payload_identify_file = fopen("/tmp/traj3withload.txt", "w+");
    //         }
    //         else
    //         {
    //             payload_identify_file = fopen("/tmp/traj3withoutload.txt", "w+");
    //         }
    //     }
    //     // payload_identify_file = fopen("/tmp/identify.txt", "w+");
    // }
    // else if (zucmotStatus->identifying_status == -1 && last_identifying_status == 1)
    // {
    //     fclose(payload_identify_file);
    //     payload_identify_file = NULL;
    //     reportError(0x1111, "error happened during payload identification");
    // }
    // else if (zucmotStatus->identifying_status == 1 && last_identifying_status == 1)
    // {
    //     if (GET_MOTION_INPOS_FLAG())
    //     {
    //         fclose(payload_identify_file);
    //         payload_identify_file = NULL;
    //         zucmotStatus->identifying_status = 2;
    //     }
    //     else if (payload_identify_file)
    //     {
    //         if (zucmotConfig->robotModel / 10000 == 1)
    //         {
    //             if (zucmotStatus->identifying_trajid == 1 || zucmotStatus->identifying_trajid == 2)
    //             {
    //                 // joint pos in rad, and torque in N.m
    //                 double jointTorq4 = (servoStatus[3].actualTorque / 8192.0) * motors[3].rated_trans_ratio * motors[3].rated_torq;
    //                 double jointTorq6 = (servoStatus[5].actualTorque / 8192.0) * motors[5].rated_trans_ratio * motors[5].rated_torq;
    //                 fprintf(payload_identify_file, "%f %f %f %f\n", joints[3].pos_fb * PM_PI / 180.0, jointTorq4, joints[5].pos_fb * PM_PI / 180.0, jointTorq6);
    //             }
    //             else if (zucmotStatus->identifying_trajid == 0)
    //             {
    //                 // joint pos in rad, and torque in N.m
    //                 double jointTorq3 = (servoStatus[2].actualTorque / 8192.0) * motors[2].rated_trans_ratio * motors[2].rated_torq;
    //                 double jointTorq5 = (servoStatus[4].actualTorque / 8192.0) * motors[4].rated_trans_ratio * motors[4].rated_torq;
    //                 fprintf(payload_identify_file, "%f %f %f %f\n", joints[2].pos_fb * PM_PI / 180.0, jointTorq3, joints[4].pos_fb * PM_PI / 180.0, jointTorq5);
    //             }
    //         }
    //         else
    //         {
    //             if (zucmotStatus->identifying_trajid == 0 || zucmotStatus->identifying_trajid == 1)
    //             {
    //                 // joint pos in rad, and torque in N.m
    //                 double jointTorq4 = (servoStatus[3].actualTorque / 8192.0) * motors[3].rated_trans_ratio * motors[3].rated_torq;
    //                 double jointTorq6 = (servoStatus[5].actualTorque / 8192.0) * motors[5].rated_trans_ratio * motors[5].rated_torq;
    //                 fprintf(payload_identify_file, "%f %f %f %f\n", joints[3].pos_fb * PM_PI / 180.0, jointTorq4, joints[5].pos_fb * PM_PI / 180.0, jointTorq6);
    //             }
    //             else if (zucmotStatus->identifying_trajid == 2)
    //             {
    //                 // joint pos in rad, and torque in N.m
    //                 double jointTorq5 = (servoStatus[4].actualTorque / 8192.0) * motors[4].rated_trans_ratio * motors[4].rated_torq;
    //                 fprintf(payload_identify_file, "%f %f\n", joints[4].pos_fb * PM_PI / 180.0, jointTorq5);
    //             }
    //         }
    //     }
    // }
    // last_identifying_status = zucmotStatus->identifying_status;

    // double q_fb[ZUCMOT_MAX_JOINTS] = {};
    // double tor_fb[ZUCMOT_MAX_JOINTS] = {};
    // double tor_sensor[ZUCMOT_MAX_JOINTS] = {};
    // for (int i = 0; i < zucmotConfig->numJoints; i++)
    // {
    //     q_fb[i] = joints[i].pos_fb * PM_PI / 180.0;
    //     tor_fb[i] = (servoStatus[i].actualTorque / 8192.0) * motors[i].rated_trans_ratio * motors[i].rated_torq;
    //     tor_sensor[i] = zucmotStatus->servo_status.jointMonitorData[i].joint_tor_sensor;
    // }
    // // record for dynamics identification, if in identifying
    // if (recordIdentifyData(zucmotStatus->dynamics_identifying_status, zucmotStatus->motion_state, q_fb, tor_fb, tor_sensor) == -1)
    // {
    //     resetIdentifyTrajPos();
    //     zucmotStatus->dynamics_identifying_status = 0;  // 未进入servo模式
    // }
    // // pos cmd for friction identify
    // friction_identify_moving();
    // recordFriciotnIdentifyData(zucmotStatus->friction_identifying_status, zucmotStatus->motion_state, q_fb, tor_fb);
}

static void update_joint_input()
{
#if 0
    for (int joint_num = 0; joint_num < zucmotConfig->numJoints; joint_num++)
    {
        // rtapi_print("joint %d = joint_pos_cmd : %f, joint->motor_pos_fb : %f, actualPosition : %f\n", joint_num, joint->pos_cmd, joint->motor_pos_fb, servoStatus[joint_num].actualPosition / DEG2ABSCNT);
        auto joint = &joints[joint_num];
        auto joint_data = &(zucmot_hal_data->joint[joint_num]);
        //added by alex at 2018.10.10 for can master
        double lastPos = joint->motor_pos_fb;
        // double lastPosfb = joint->pos_fb;
        // ServoStatus lastServoStatus = servoStatus[joint_num];

        servoStatus[joint_num] = servoRefreshStatus(joint_num);
        joint = &joints[joint_num];
        if (!GET_JOINT_ACTIVE_FLAG(joint))
        {
            continue;
        }
        zucmotStatus->servo_collision_base[joint_num] = servoStatus[joint_num].collision_base;
        zucmotStatus->servo_collision_level[joint_num] = servoStatus[joint_num].collision_level;
        joint->backdrive_state = servoStatus[joint_num].backDriveState;
        /* copy data from HAL to joint structure */
        joint->index_enable = (joint_data->index_enable);
        //joint->motor_pos_fb = *(joint_data->motor_pos_fb);
        // 伺服底层会进行补偿，这里不再补偿 @FIXME liwang 验证zu系列
        joint->motor_pos_fb =
            servoStatus[joint_num].actualPosition / DEG2ABSCNT + zucmotStatus->dhParam.joint_homeoff[joint_num]; /*compensate the home offset*/

        if (servoStatus[joint_num].connectState == 0)  // servo is not power on
        {
            joint->motor_pos_fb = lastPos;
        }

        joint->abs_pos_fb = servoStatus[joint_num].actualPosition;
        /* calculate pos_fb */
        if ((joint->home_state == HOME_INDEX_SEARCH_WAIT) && (joint->index_enable == 0))
        {
            joint->pos_fb = joint->pos_cmd;
        }
        else
        {
            /* normal case: subtract backlash comp and motor offset */
            joint->pos_fb = joint->motor_pos_fb - (joint->backlash_filt);
        }

        /* calculate following error */
        joint->ferror = joint->pos_cmd - joint->pos_fb;
        joint->ferror = 0.0;
        auto abs_ferror = fabs(joint->ferror);
        /* update maximum ferror if needed */
        if (abs_ferror > joint->ferror_high_mark)
        {
            joint->ferror_high_mark = abs_ferror;
        }

        /* calculate following error limit */
        if (joint->vel_limit > 0.0)
        {
            joint->ferror_limit = joint->max_ferror * fabs(joint->vel_cmd) / joint->vel_limit;
        }
        else
        {
            joint->ferror_limit = 0;
        }
        if (joint->ferror_limit < joint->min_ferror)
        {
            joint->ferror_limit = joint->min_ferror;
        }
        /* update following error flag */
        if (!(zucmot_hal_data->drag_mode) && (zucmot_hal_data->motors_servo_on) && (abs_ferror > joint->ferror_limit))
        {
            SET_JOINT_FERROR_FLAG(joint, 1);
            rtapi_print("following error details: jnt->pos_cmd = %f, jnt->pos_fb = %f, abs_ferror = %f, ferror_limit = %f\n",
                        joint->pos_cmd,
                        joint->pos_fb,
                        abs_ferror,
                        joint->ferror_limit);
        }
        else
        {
            SET_JOINT_FERROR_FLAG(joint, 0);
        }

        /* read limit switches */
        if ((joint_data->pos_lim_sw))
        {
            SET_JOINT_PHL_FLAG(joint, 1);
        }
        else
        {
            SET_JOINT_PHL_FLAG(joint, 0);
        }
        if ((joint_data->neg_lim_sw))
        {
            SET_JOINT_NHL_FLAG(joint, 1);
        }
        else
        {
            SET_JOINT_NHL_FLAG(joint, 0);
        }
        joint->on_pos_limit = GET_JOINT_PHL_FLAG(joint);
        joint->on_neg_limit = GET_JOINT_NHL_FLAG(joint);
        /* read amp fault input */
        if ((joint_data->amp_fault))
        {
            SET_JOINT_FAULT_FLAG(joint, 1);
        }
        else
        {
            SET_JOINT_FAULT_FLAG(joint, 0);
        }

        /* read home switch input */
        if ((joint_data->home_sw))
        {
            SET_JOINT_HOME_SWITCH_FLAG(joint, 1);
        }
        else
        {
            SET_JOINT_HOME_SWITCH_FLAG(joint, 0);
        }
        /* end of read and process joint inputs loop */
        // rtapi_print("joint %d = joint_pos_cmd : %f, joint->motor_pos_fb : %f, actualPosition : %f\n", joint_num, joint->pos_cmd, joint->motor_pos_fb, servoStatus[joint_num].actualPosition / DEG2ABSCNT);
    }
#endif
}
static void update_feed_scale()
{
    unsigned char enables = 0;
    /* compute net feed and spindle scale factors */
    if (zucmotStatus->motion_state == ZUCMOT_MOTION_COORD)
    {
        enables = zucmotStatus->enables_queued;
    }
    else
    {
        enables = zucmotStatus->enables_new;
    }

    /* feed scaling first:  feed_scale*/
    double scale = 1.0;
    if ((zucmotStatus->motion_state != ZUCMOT_MOTION_FREE) && (zucmotStatus->motion_state != ZUCMOT_MOTION_TELEOP) &&
        (zucmotStatus->motion_state != ZUCMOT_MOTION_TELEOP_TOOL) && (enables & FS_ENABLED))
    {
        scale *= zucmotStatus->rapid_scale;
    }
    /* save the resulting combined scale factor */
    zucmotStatus->net_feed_scale = scale;
}
static void update_tio_status()
{
    /* read tio status */
    TIOStatus new_tio_status = tio_status();
    if ((new_tio_status.tio_version & 0xFFFF) >= 0x0300 && tioStatus.tio_version == 0)
    {
        synTIOConfigsForHwV3();  // only for hardware v3
    }
    if ((zucmotConfig->safetyCfg.motRunMode == RUN_MODE_SIM_ON_REAL || zucmotConfig->safetyCfg.motRunMode == RUN_MODE_SIM_WITHOUT_CAN))
    {
        new_tio_status.Lamp_color = get_tio_settings().Lamp_color;
    }
    if (!zucmotStatus->powered_on)
    {
        new_tio_status.Lamp_color = 0;
    }
    if (tioStatus.tio_version != new_tio_status.tio_version)
    {
        rtapi_print("-----TIO VERSION %2x-----\n", new_tio_status.tio_version);
    }
    tioStatus = new_tio_status;
}
static void process_inputs(void)
{
    //check safety io
    safety::check_safety_io();

    // //急停信号处理
    // zucmotStatus->emergency_stop = safety::SafeIOMgr::instance().estop_ev()->is_safe();
    // estop_reaction(zucmotStatus->emergency_stop);

    /*check function io*/
    funcio::check_func_io();

    //更新轴组
    update_axisgroup_fsm();

    handle_category_one_stop();  //急停程序暂停的处理。FIXME：后面这块交由任务层采用第二序列的方法处理

    // update_identifying_status();

    update_joint_input();

    update_feed_scale();
}

static void motAbortMotion()
{
#if 0
    int axis_num = 0;
    int joint_num = 0;
    zucmot_joint_t* joint;
    zucmot_axis_t* axis;

    /* zucMotionAbort(); */
    for (axis_num = 0; axis_num < CART_DIM; axis_num++)
    {
        axis = &axes[axis_num];
        axis->teleop_tp.enable = 0;
        // axis->teleop_tp.curr_vel = 0.0;
        axis->teleop_tool_tp.enable = 0;
        // axis->teleop_tool_tp.curr_vel = 0.0;
    }

    for (joint_num = 0; joint_num < zucmotConfig->numJoints; joint_num++)
    {
        joint = &joints[joint_num];
        joint->free_tp.enable = 0;
        // joint->free_tp.curr_pos = joint->pos_cmd;
        // joint->free_tp.curr_vel = 0.0;
        // if ( joint->home_state != HOME_IDLE ) {
        // 	joint->home_state = HOME_ABORT;
        // }
    }
    // rtapi_print(" ##################################### collision_rebound motAbortMotion ##################################### \n");
//    tpManager_tppause();
    //if (GET_MOTION_COORD_FLAG())
    // {
//    // 	tpManager_tpabort();
    // }

    /* zucTrajDisable()*/
    // zucmotDebug->coordinating = 0;
    // zucmotDebug->teleoperating = 0;
    // zucmotDebug->teleoperating_tool = 0;
    // zucmotDebug->servomoveoperating = 0;
    zucmotDebug->dragging = 0;
    if (GET_MOTION_TELEOP_FLAG())
    {
        for (axis_num = 0; axis_num < CART_DIM; axis_num++)
        {
            axes[axis_num].teleop_tp.enable = 0;
            if (zucmotStatus->pre_check_joint_simple_tp == 1)
            {
                axes[axis_num].teleop_tp.max_acc = (axis_num < 3) ? PRE_CHECK_STOP_POS_ACC : PRE_CHECK_STOP_ORI_ACC;
            }
            else
            {
                axes[axis_num].teleop_tp.max_acc = (axis_num < 3) ? 3000.0 : 1080.0;
            }
        }
    }
    else if (GET_MOTION_TELEOP_TOOL_FLAG())
    {
        for (axis_num = 0; axis_num < CART_DIM; axis_num++)
        {
            axes[axis_num].teleop_tool_tp.enable = 0;
            if (zucmotStatus->pre_check_joint_simple_tp == 1)
            {
                axes[axis_num].teleop_tool_tp.max_acc = (axis_num < 3) ? PRE_CHECK_STOP_POS_ACC : PRE_CHECK_STOP_ORI_ACC;
            }
            else
            {
                axes[axis_num].teleop_tool_tp.max_acc = (axis_num < 3) ? 3000.0 : 1080.0;
            }
        }
    }

    if (zucmotStatus->identifying_status == 1)
    {
        zucmotStatus->identifying_status = -1;
    }
#endif
}

static void servoErrNotHasCategoryHandle(int errcode, zucmot_debug_t* zucmotDebug, int errCategory)
{
    if (errcode != SERVO_ERR_COLLISION && errcode != ENC_OVERTEMP && errcode != POS_DEVIATION_WARNING)
    {
        SET_MOTION_ERROR_FLAG(1);
        // rtapi_print(" ################################### [collision_rebound 222] ################################### \n");
        motAbortMotion();
        zucmotDebug->enabling = 0;
        zucmotDebug->dragging = 0;
        //        tpManager_tpabort();
    }
}

static void servoErrHasCategoryHandle(int errcode, zucmot_debug_t* zucmotDebug, int errCategory)
{
    if (errcode != SERVO_ERR_COLLISION && errcode != ENC_OVERTEMP && errcode != POS_DEVIATION_WARNING)
    {
        /* added by yf, avoid setting error flag in resume mode after collision detection */
        if (zucmotConfig->safetyCfg.clsnOpt.reactionType && (errcode == POS_DEVIATION_ERR || zucmotStatus->protective_stop)) {}
        else
        {
            SET_MOTION_ERROR_FLAG(1);
        }
        // rtapi_print(" ################################### [collision_rebound 333] ################################### \n");
        motAbortMotion();
        if (errCategory == 0 || errCategory == 1)
        {
            zucmotDebug->enabling = 0;
            zucmotDebug->dragging = 0;
            //            tpManager_tpabort();
        }
        // if(errCategory == 2)
        // {
        //     motAbortMotion();
        // 	zucmotDebug->coordinating = 0;
        // 	SET_MOTION_ERROR_FLAG(1);
        // }
    }
}

static int Quaternions_interpolation_angular_vel_calculation(PmQuaternion const* const quaternions_last,
                                                             PmQuaternion const* const quaternions_new,
                                                             double period,
                                                             double* const Angular_Vel,
                                                             PmRotationVector* const Delt_Rot_aix)
{
    PmQuaternion quaternions_last_inv, quatDelt;
    pmQuatInv(quaternions_last, &quaternions_last_inv);
    pmQuatQuatMult(quaternions_new, &quaternions_last_inv, &quatDelt);
    if (quatDelt.s > 1.0)
        quatDelt.s = 1.0;
    else if (quatDelt.s < -1.0)
        quatDelt.s = -1.0;
    double tmag = fabs(2 * acos(quatDelt.s) * 180.0 / PM_PI);
    double theta_quat = acos(quatDelt.s);
    *Angular_Vel = tmag / (period);
    Delt_Rot_aix->s = tmag;
    if (fabs(sin(theta_quat) < 1e-6))
    {
        Delt_Rot_aix->x = 0;
        Delt_Rot_aix->y = 0;
        Delt_Rot_aix->z = 0;
    }
    else
    {
        Delt_Rot_aix->x = quatDelt.x / sin(theta_quat);
        Delt_Rot_aix->y = quatDelt.y / sin(theta_quat);
        Delt_Rot_aix->z = quatDelt.z / sin(theta_quat);
    }
    return 0;
}

void zucmotStatReset_NoMotion()
{
    zucmotStatus->distance_to_go = 0;
    zucmotStatus->enables_queued = zucmotStatus->enables_new;
    zucmotStatus->requested_vel = 0;
    zucmotStatus->current_vel = 0;
    zucPoseZero(&zucmotStatus->dtg);
}

static int checkSafezone_init_control(zucmot_status_t* zucmotStatus_checkSafezone)
{
    zucmotStatus_checkSafezone->pre_check_joint_simple_tp = 0;
    zucmotStatus_checkSafezone->check_safezone_prediction = 0;
    zucmotStatus_checkSafezone->check_singular_7dof = 0;
    zucmotStatus_checkSafezone->safezone_dengrous = 0;
    zucmotStatus_checkSafezone->safezone_dengrous = checkSafezone_dangerous(zucmotConfig->safetyCfg.zone,
                                                                            zucmotStatus->carte_pos_cmd,
                                                                            zucmotStatus->carte_elbow_pos_cmd,
                                                                            zucmotConfig->safetyCfg.zone.elbow_pos_limit_enable,
                                                                            zucmotConfig->safetyCfg.elbow_sphere_radius,
                                                                            zucmotConfig->user_offset);
    return 0;
}

static void get_pos_cmds()
{
    int joint_num, axis_num, result;
    zucmot_joint_t* joint;
    zucmot_axis_t* axis;
    double positions[ZUCMOT_MAX_JOINTS];
    double last_pos_cmds[ZUCMOT_MAX_JOINTS];
    double vel_lim;
    int motion_type = TC_NONE, next_motion_type = TC_NONE, parabolic_blending = 0;
    int onlimit = 0;
    static int pre_check_limit = 0;
    int joint_limit[ZUCMOT_MAX_JOINTS][2] = {0};
    singularity_check_flag = 0;
    double old_jnts[ZUCMOT_MAX_JOINTS] = {0};
    int first_cycle_disable_cct = 0;
    int attitude_enable = 0;
    // 将关节角度 赋值到本地数组
    last_carte_pos_cmd = zucmotStatus->carte_pos_cmd;
    zucmotStatus->last_last_carte_pos_cmd = zucmotStatus->last_carte_pos_cmd;
    zucmotStatus->last_carte_pos_cmd = zucmotStatus->carte_pos_cmd;
    //    // tpManager_tpExtUpdateInterpolation();
    for (joint_num = 0; joint_num < zucmotConfig->numJoints; joint_num++)
    {
        joint = &joints[joint_num];
        positions[joint_num] = joint->pos_cmd;
        last_pos_cmds[joint_num] = joint->pos_cmd;
    }
    // 如果没有用完9个轴，则未用到的轴置0
    while (joint_num < ZUCMOT_MAX_JOINTS) { positions[joint_num++] = 0.0; }

    if (zucmotStatus->status_acc_convyr == 3 && first_case3)
    {
        // first_case3 = 0;
        // rtapi_print("Finish speed synchronization, and the current positions are :\n");
        // rtapi_print("[get_pos_cmds] reality_joint = %f %f %f %f %f %f\n",
        //             last_pos_cmds[0],
        //             last_pos_cmds[1],
        //             last_pos_cmds[2],
        //             last_pos_cmds[3],
        //             last_pos_cmds[4],
        //             last_pos_cmds[5]);
        // double jPos[ZUCMOT_MAX_JOINTS];
        // //        tpManager_tpGetCurJointPos(jPos);
        // rtapi_print("[get_pos_cmds] derired_joint = %f %f %f %f %f %f\n", jPos[0], jPos[1], jPos[2], jPos[3], jPos[4], jPos[5]);
    }
    checkSafezone_init_control(zucmotStatus);
    // 启动运动控制计算 根据状态运行轨迹规划程序
}

int getErrorPrioJointNum()
{
    // int i = 0;
    // int num = -1;

    // for (i = 0; i < zucmotConfig->numJoints; i++)
    // {
    //     if (0 == status[i].errorPriority)
    //     {
    //         num = i;
    //     }
    //     status[i].errorPriority -= 1;
    // }
    // return num;
    return -1;
}

static void handle_servo_error()
{
    // int axis_num = 0;
    int joint_num = 0;
    zucmot_joint_t* joint;
    // zucmot_axis_t *axis;
    // unsigned int errType = 0;
    static int over_temp_is_new = 1;
    static int counter_20s;
    static int counter_5m;
    // static UINT32 last_errorCode[ZUCMOT_MAX_JOINTS] = {0};     //latest error code of each axis
    static UINT8 last_connectState[ZUCMOT_MAX_JOINTS] = {0};  //latest connection state  of each axis
    static UINT32 last_errorCodeAll = 0;                      //high word: joint_num, low word: error code
    static UINT8 last_enable_state = 0;
    // static int send_reset = 0;
    static int error_state = 0;  //遇到需要进行Cat.1 Stop 处理的error时对此状态置位
    // SCBStatus scbStatus;
    static int last_scb_errCode;
    static int last_scb_estop_signal_abnormal = 0;
    counter_20s++;
    int sem_id;
    static int count = 0;
    static int last_enable = 0;

    double aborting_jntPos[ZUCMOT_MAX_JOINTS] = {0.0};  // joint position in rad
    double aborting_jntVel[ZUCMOT_MAX_JOINTS] = {0.0};  // joint speed in rad/s
    // double aborting_jntAcc[ZUCMOT_MAX_JOINTS] = {0.0};		// joint speed in rad/s^2
    static double aborting_lastJntPos[ZUCMOT_MAX_JOINTS] = {0.0};
    // static double aborting_lastJntVel[ZUCMOT_MAX_JOINTS] = {0.0};
    static int aborting_flag[ZUCMOT_MAX_JOINTS] = {0};
    // static double aborting_fb[ZUCMOT_MAX_JOINTS] = {0.0};
    // static double bounce_pos_cmd[ZUCMOT_MAX_JOINTS] = {0.0};
    // static double FT_fil[ZUCMOT_MAX_JOINTS] = {0.0};
    // static double wbase[ZUCMOT_MAX_JOINTS] = {0.0};

    for (joint_num = 0; joint_num < zucmotConfig->numJoints; ++joint_num)
    {
        aborting_jntPos[joint_num] = joints[joint_num].pos_cmd * PM_PI / 180.0;
        aborting_jntVel[joint_num] = (aborting_jntPos[joint_num] - aborting_lastJntPos[joint_num]) / zucmotConfig->trajCycleTime;
        // aborting_jntAcc[joint_num] = (aborting_jntVel[joint_num]-aborting_lastJntVel[joint_num])/zucmotConfig->trajCycleTime;
        aborting_lastJntPos[joint_num] = aborting_jntPos[joint_num];
        // aborting_lastJntVel[joint_num] = aborting_jntVel[joint_num];
    }
    // 碰撞检测主函数
    // static int last_mode = 0;
    // static SaftyMonitorRes last_safty_monitor_res = SaftyMonitorRes::NO_ALRAM;
    // SaftyMonitorRes safty_monitor_res = NO_ALRAM;

    // // safty_monitor_res = SaftyAction(last_mode, joints, zucmotConfig, servoStatus, zucmotSensorStruct, zucmotStatus, externFtBsensor);
    // safty_monitor_res = SaftyAction(last_mode, joints, zucmotConfig, nullptr, zucmotSensorStruct, zucmotStatus, externFtBsensor);

    /**
 * 临时处理急停后第一次上使能积分未归零的问题
 * 
*/
    // if (!last_enable && GET_MOTION_ENABLE_FLAG())
    // {
    //     count = 20;
    // }
    // if (count > 0)
    // {
    //     count--;
    // }
    // if (!count)
    // {
    //     // 检测到碰撞后响应动作
    //     if (safty_monitor_res == MOMENTUM_ALRAM || safty_monitor_res == POWER_ALARM)
    //     {
    //         // 屏蔽发生碰撞时和碰撞反弹时的报警，避免调使能造成二次碰撞
    //    //         if (!(tpManager_tpCollision_rebound() == 1 || tpManager_tpCollision_rebound() == 2 || tpManager_tpCollision_rebound() == 5))
    //         {
    //             if (last_safty_monitor_res != safty_monitor_res)
    //             {
    //                 if (safty_monitor_res == MOMENTUM_ALRAM)
    //                 {
    //                     reportError(FUNCTION_SAFTY_MOMENTUM_EXCEED_LIMIT, "robot momentum exceeds the limit");
    //                     rtapi_print("robot momentum exceeds the limit, %lf, %lf\n", zucmotStatus->robot_momentum, zucmotStatus->robot_momentum_limit[1]);
    //                     zucmotStatus->power_protective_stop = 1;
    //                 }
    //                 else
    //                 {
    //                     reportError(FUNCTION_SAFTY_POWER_EXCEED_LIMIT, "robot power exceeds the limit");
    //                     rtapi_print("robot power exceeds the limit, %lf, %lf\n", zucmotStatus->robot_power, zucmotStatus->robot_power_limit[1]);
    //                     zucmotStatus->momentum_protective_stop = 1;
    //                 }
    //    //                 tpManager_tpabort();
    //                 motAbortMotion();
    //                 SET_MOTION_ERROR_FLAG(1);
    //                 SET_MOTION_ENABLE_FLAG(0);  // 下使能
    //             }
    //         }
    //     }
    //     else if (safty_monitor_res != NO_ALRAM)
    //     {
    //    //         if (!last_safty_monitor_res && last_safty_monitor_res != safty_monitor_res && tpManager_tpCollision_rebound() == 0)
    //         {
    //             switch (safty_monitor_res)
    //             {
    //             case BASE_ALARM:
    //                 reportError(COLLISION_PROTECTION, "robot detectes collision by base sensor #{\"fixed_key\":[\"BASE\"]}");
    //                 break;
    //             case SERVO_ALRAM_JOINT1:
    //             case SERVO_ALRAM_JOINT2:
    //             case SERVO_ALRAM_JOINT3:
    //             case SERVO_ALRAM_JOINT4:
    //             case SERVO_ALRAM_JOINT5:
    //             case SERVO_ALRAM_JOINT6: {
    //                 reportError(COLLISION_PROTECTION,
    //                             "robot detectes collision by servo #{\"fixed_key\":[\"JOINTP : %d\"]}",
    //                             safty_monitor_res - SERVO_ALRAM_JOINT1 + 1);
    //                 break;
    //             }
    //             case GM_ALRAM_JOINT1:
    //             case GM_ALRAM_JOINT2:
    //             case GM_ALRAM_JOINT3:
    //             case GM_ALRAM_JOINT4:
    //             case GM_ALRAM_JOINT5:
    //             case GM_ALRAM_JOINT6:
    //                 reportError(
    //                     COLLISION_PROTECTION, "robot detectes collision by joint #{\"fixed_key\":[\"JOINTG : %d\"]}", safty_monitor_res - GM_ALRAM_JOINT1 + 1);
    //                 break;
    //             default:
    //                 rtapi_print("safty_monitor_res Unknown: %d\n", safty_monitor_res);
    //                 break;
    //             }
    //             rtapi_print("robot detectes collision by %d\n", safty_monitor_res);
    //    //             if (tpManager_tpCollision_rebound() != 5 && tpManager_tpCollision_rebound() != 1 && tpManager_tpCollision_rebound() != 2)
    //             {
    //                 if (zucmotStatus->protective_stop == 0)
    //                 {
    //                     zucmotStatus->protective_stop = 1;
    //                     (zucmot_hal_data->error_triggered) = 1;
    //                     (zucmot_hal_data->motion_error_code) = COLLISION_PROTECTION;

    //                     collision_detected_in_movement = GET_MOTION_INPOS_FLAG() ? 0 : 1;

    //                     if (zucmotConfig->safetyCfg.clsnOpt.reactionType == 1)
    //                     {  // no rebound
    //                         rtapi_print("collision_detected_in_movement = %d, pausing\n", collision_detected_in_movement);
    //                     }
    //                     else
    //                     {
    //    //                         tpManager_tpabort();
    //                         motAbortMotion();
    //                         SET_MOTION_ERROR_FLAG(1);
    //                         if (collision_detected_in_movement)
    //                         {
    //                             // rebound
    //                             rtapi_print("qd : %lf %lf %lf %lf %lf %lf\n",
    //                                         _robot_val.jntVelCmd[0],
    //                                         _robot_val.jntVelCmd[1],
    //                                         _robot_val.jntVelCmd[2],
    //                                         _robot_val.jntVelCmd[3],
    //                                         _robot_val.jntVelCmd[4],
    //                                         _robot_val.jntVelCmd[5]);
    //                             for (int joint_num = 0; joint_num < zucmotConfig->numJoints; joint_num++)
    //                             {
    //                                 if (_robot_val.jntVelCmd[joint_num] > Q_FUZZ)
    //                                 {
    //                                     zucmotStatus->vel_direction[joint_num] = 1;
    //                                 }
    //                                 else if (_robot_val.jntVelCmd[joint_num] < -Q_FUZZ)
    //                                 {
    //                                     zucmotStatus->vel_direction[joint_num] = -1;
    //                                 }
    //                                 else
    //                                 {
    //                                     zucmotStatus->vel_direction[joint_num] = 0;
    //                                 }
    //                             }
    //                             // 非力控时，才可以执行反弹
    //                             // 补充，反弹角度、反弹长度为0时，不进行rebound_pos的计算，避免计算浪费；
    //                             if (zucmotStatus->compliantType == 0 &&
    //                                 (zucmotConfig->safetyCfg.clsnOpt.reboundAngle > Q_FUZZ || zucmotConfig->safetyCfg.clsnOpt.reboundLength > Q_FUZZ))
    //                             {
    //    //                                 int ret_info = tpManager_get_rebound_pos();
    //                                 if (ret_info == 1)
    //                                 {
    //                                     reportInfo(INFO_COLLISION_REBOUND_ABORT_STOP, "robot already stopped, no rebound follows");
    //                                 }
    //                                 else if (ret_info == 2)
    //                                 {
    //                                     reportInfo(INFO_COLLISION_REBOUND_ABORT, "collision rebound may abort");
    //                                 }

    //                                 for (int i = 0; i < zucmotConfig->numJoints; i++) { set_robot_joint_avoid_pos_mutation(i, 200); }
    //                             }
    //                         }
    //                         else
    //                         {
    //                             reportInfo(INFO_COLLISION_REBOUND_ABORT_STOP, "robot already stopped, no rebound follows");
    //                         }
    //                     }
    //                 }
    //                 else if (collision_detected_in_movement == 1)
    //                 {
    //                     rtapi_print("collision is being handling \n");
    //                 }
    //             }
    //         }
    //     }
    // }

    // last_enable = GET_MOTION_ENABLE_FLAG();
    // last_safty_monitor_res = safty_monitor_res;

    if (zucmotSensorStruct->sensorConfig.torqsensorBrand == 4)
    {
        int sem_id = sem_get(SENSOR_SEM_KEY);
        if (sem_p(sem_id) == 0)
        {
            for (int joint_num = 0; joint_num < CART_DIM; joint_num++)
            {
                zucmotSensorStruct->sensorStatus.actTorque[joint_num] = externFtBsensor[joint_num];  // 更新底座传感器数据
            }
            sem_v(sem_id);
        }
    }

    // rtapi_print("FT_fil[0] = %lf, wbase[0] = %lf	%lf\n", FT_fil[0], wbase[0] ,zucmotSensorStruct->sensorStatus.torque[0]);
    // rtapi_print("FT_fil[1] = %lf, wbase[1] = %lf	%lf\n", FT_fil[1], wbase[1] ,zucmotSensorStruct->sensorStatus.torque[1]);
    // rtapi_print("FT_fil[2] = %lf, wbase[2] = %lf	%lf\n", FT_fil[2], wbase[2] ,zucmotSensorStruct->sensorStatus.torque[2]);
    // rtapi_print("FT_fil[3] = %lf, wbase[3] = %lf	%lf\n", FT_fil[3], wbase[3] ,zucmotSensorStruct->sensorStatus.torque[3]);
    // rtapi_print("FT_fil[4] = %lf, wbase[4] = %lf	%lf\n", FT_fil[4], wbase[4] ,zucmotSensorStruct->sensorStatus.torque[4]);
    // rtapi_print("FT_fil[5] = %lf, wbase[5] = %lf	%lf\n", FT_fil[5], wbase[5] ,zucmotSensorStruct->sensorStatus.torque[5]);

    if (zucmotSensorStruct->sensorConfig.torqsensorBrand != 4 && zucmotStatus->motion_state != ZUCMOT_MOTION_ADMITTANCE && zucmotStatus->compliantType == 0 &&
        zucmotSensorStruct->sensorStatus.status == 1)
    {
        double cartPos[CART_DIM] = {0.0};
        double FT_act[CART_DIM] = {0.0};
        // 计算传感器中心位姿	@FIXME
        zucmot_joint_t* joint = NULL;
        double joint_fb[ZUCMOT_MAX_JOINTS] = {0.0};
        // double joint_cmd[ZUCMOT_MAX_JOINTS] = {0.0};
        for (joint_num = 0; joint_num < zucmotConfig->numJoints; joint_num++)
        {
            joint = &joints[joint_num];
            joint_fb[joint_num] = joint->pos_fb;
            // joint_cmd[joint_num] = joint->pos_cmd;
        }
        KINEMATICS_FORWARD_FLAGS temp_fflags = ctrl_fflags;
        KINEMATICS_INVERSE_FLAGS temp_iflags = ctrl_iflags;
        ZucPose sensor_CartPos = {{0.0}};
        ZucPose tool_offset = {{0.0}};  // @FIXME  当前默认传感器安装与末端法兰一致
        kinematicsForward(&tool_offset, joint_fb, &sensor_CartPos, &temp_fflags, &temp_iflags, &zucmotConfig->base_offset, &zucmotConfig->user_offset);
        cartPos[0] = sensor_CartPos.tran.x;
        cartPos[1] = sensor_CartPos.tran.y;
        cartPos[2] = sensor_CartPos.tran.z;
        cartPos[3] = sensor_CartPos.a * PM_PI / 180.0;
        cartPos[4] = sensor_CartPos.b * PM_PI / 180.0;
        cartPos[5] = sensor_CartPos.c * PM_PI / 180.0;

        double torque[CART_DIM] = {0.0};
        int sem_id = sem_get(SENSOR_SEM_KEY);
        if (sem_p(sem_id) == 0)
        {
            for (int i = 0; i < CART_DIM; i++) { torque[i] = zucmotSensorStruct->sensorStatus.torque[i]; }
            sem_v(sem_id);
        }
        axisgroup::AxisGroupMgr::instance().get_axisgroup(0)->dyna()->get_Fact(
            cartPos, torque, FT_act, zucmotStatus->toolPayLoad.m, zucmotStatus->toolPayLoad.p, 0);
        if (zucmotStatus->compliantEnable)
        {  // 勾选初始化，上升沿计算偏置，该值为外力
            for (int i = 0; i < CART_DIM; i++)
            {
                zucmotSensorStruct->sensorStatus.actTorque[i] = FT_act[i];
                zucmotStatus->actualTorque[i] = FT_act[i];
            }
        }
        else
        {  // 勾选不初始化，该值为传感器的力
            int sem_id = sem_get(SENSOR_SEM_KEY);
            if (sem_p(sem_id) == 0)
            {
                for (int i = 0; i < CART_DIM; i++) { zucmotSensorStruct->sensorStatus.actTorque[i] = zucmotSensorStruct->sensorStatus.torque[i]; }
                sem_v(sem_id);
            }
        }
        // rtapi_print("zucmotSensorStruct->sensorStatus.torque = %lf\n", zucmotSensorStruct->sensorStatus.torque[0]);
        // rtapi_print("zucmotSensorStruct->sensorStatus.actTorque = %lf\n", zucmotSensorStruct->sensorStatus.actTorque[0]);
    }

    // if (zucmotStatus->motion_state != ZUCMOT_MOTION_ADMITTANCE)
    // {
    //     last_mode = 0;
    // }
    // else
    // {
    //     last_mode = 1;
    // }

    if (zucmotStatus->category_one_stop_program_pause == 0 && last_enable_state && !GET_MOTION_ENABLE_FLAG() && zucmotDebug->enabling)
    {
        motAbortMotion();
        //        tpManager_tpclear();
        zucmotStatus->last_momentum_movescale = 1.0;
        zucmotStatus->last_power_movescale = 1.0;
        zucmotStatus->momvelscale = 1.0;
        ZERO_ZUC_POSE(zucmotStatus->dtg);
        SET_MOTION_INPOS_FLAG(1);
        zucmotDebug->enabling = 0;
        zucmotDebug->dragging = 0;
        SET_MOTION_ERROR_FLAG(1);
        axisgroup::AxisGroupMgr::instance().get_axisgroup(0)->do_enable(false);
        rtapi_print("SERVO enable off for unknown reason \n");
    }
    last_enable_state = GET_MOTION_ENABLE_FLAG();
    canmonitor_record_timer(25);

    //处理关节伺服报错
    // for (joint_num = 0; joint_num < zucmotConfig->numJoints; joint_num++)
    // {
    //     canmonitor_record_timer(30 + joint_num);
    //     int joint_num;
    //     joint_num = getErrorPrioJointNum();

    //     if (servoStatus[joint_num].errorCode)
    //     {
    //         /*handle root error and report to client*/
    //         if (last_errorCodeAll == 0 || last_errorCodeAll != (joint_num << ERROR_OFFSET_FOR_JOINT_NUM) + servoStatus[joint_num].errorCode)
    //         {
    //             rtapi_print("servoStatus[%d].errorCode is %x\n", joint_num, servoStatus[joint_num].errorCode);
    //             last_errorCodeAll = (joint_num << ERROR_OFFSET_FOR_JOINT_NUM) + servoStatus[joint_num].errorCode;
    //             rtapi_print("###last_errorCodeAll set to  %x\n", last_errorCodeAll);

    //             // /*handle non-collision error immediately*/
    //             // if (ENC_OVERTEMP == servoStatus[joint_num].errorCode && 0 == over_temp_is_new)
    //             // {
    //             //     counter_5m++;
    //             //     if (counter_20s >= TIMER_20S)
    //             //     {
    //             //         over_temp_is_new = 1;
    //             //         counter_5m = 0;
    //             //     }
    //             //     counter_20s = 0;
    //             //     if (counter_5m >= TIMER_5MIN)
    //             //     {
    //             //         over_temp_is_new = 1;
    //             //         counter_5m = 0;
    //             //     }
    //             // }

    //             ServoErrProcStrategy servoErrProc;
    //    //             if (tpManager_tpCollision_rebound() != 5 && tpManager_tpCollision_rebound() != 1 && tpManager_tpCollision_rebound() != 2)
    //             {
    //                 if (servoStatus[joint_num].hasServoErrCategory)
    //                 {
    //                     servoErrProc = servoErrHasCategoryHandle;
    //                 }
    //                 else
    //                 {
    //                     servoErrProc = servoErrNotHasCategoryHandle;
    //                 }

    //                 servoErrProc(servoStatus[joint_num].errorCode, zucmotDebug, servoStatus[joint_num].servoErrCategory);
    //             }

    //             /*report the first error immediately*/
    //             switch (servoStatus[joint_num].errorCode)
    //             {
    //             case SERVO_ERR_COLLISION:
    //                 break;
    //             case PHASE_OVERCURRENT:
    //                 reportError(DRIVE_ERR_PHASE_OVERCURRENT_J1 + (joint_num << ERROR_OFFSET_FOR_JOINT_NUM),
    //                             _("joint %d phase current over limit!\n"),
    //                             ONE_BASED(joint_num));
    //                 break;
    //             case ATUNE_OVERCURRENT:
    //                 reportError(DRIVE_ERR_ATUNE_OVERCURRENT_J1 + (joint_num << ERROR_OFFSET_FOR_JOINT_NUM),
    //                             _("joint %d auto tune current over limit!\n"),
    //                             ONE_BASED(joint_num));
    //                 break;
    //             case SAME_DIR_OVERSPD:
    //                 reportError(DRIVE_ERR_SAME_DIR_OVERSPD_J1 + (joint_num << ERROR_OFFSET_FOR_JOINT_NUM),
    //                             _("joint %d same direction over speed!\n"),
    //                             ONE_BASED(joint_num));
    //                 break;
    //             case REV_DIR_OVERSPD:
    //                 reportError(DRIVE_ERR_REV_DIR_OVERSPD_J1 + (joint_num << ERROR_OFFSET_FOR_JOINT_NUM),
    //                             _("joint %d reverse direction over speed!\n"),
    //                             ONE_BASED(joint_num));
    //                 break;
    //             case VOER_MAX_SPD:
    //                 reportError(DRIVE_ERR_OVER_MAX_SPD_J1 + (joint_num << ERROR_OFFSET_FOR_JOINT_NUM), _("joint %d over max speed!\n"), ONE_BASED(joint_num));
    //                 break;
    //             case SPD_LAG_ERR:
    //                 reportError(
    //                     DRIVE_ERR_SPD_LAG_ERR_J1 + (joint_num << ERROR_OFFSET_FOR_JOINT_NUM), _("joint %d speed err too large !\n"), ONE_BASED(joint_num));
    //                 break;
    //             case SPD_STALL_ERR:
    //                 reportError(
    //                     DRIVE_ERR_SPD_STALL_ERR_J1 + (joint_num << ERROR_OFFSET_FOR_JOINT_NUM), _("joint %d speed stall error !\n"), ONE_BASED(joint_num));
    //                 break;
    //             case ENC_TIMEOUT:
    //                 reportError(DRIVE_ERR_ENC_TIMEOUT_J1 + (joint_num << ERROR_OFFSET_FOR_JOINT_NUM),
    //                             _("joint %d encoder connetion time out!\n"),
    //                             ONE_BASED(joint_num));
    //                 break;
    //             case ENC_COUNT_ERR:
    //                 reportError(
    //                     DRIVE_ERR_ENC_COUNT_ERR_J1 + (joint_num << ERROR_OFFSET_FOR_JOINT_NUM), _("joint %d encoder count error!\n"), ONE_BASED(joint_num));
    //                 break;
    //             case POS_LAG_ERR:
    //                 reportError(
    //                     DRIVE_ERR_POS_LAG_ERR_J1 + (joint_num << ERROR_OFFSET_FOR_JOINT_NUM), _("joint %d position error too large!\n"), ONE_BASED(joint_num));
    //                 break;
    //             case POS_INC_OVER_LMT:
    //                 reportError(DRIVE_ERR_POS_INC_OVER_LMT_J1 + (joint_num << ERROR_OFFSET_FOR_JOINT_NUM),
    //                             _("joint %d position increment over limit!\n"),
    //                             ONE_BASED(joint_num));
    //                 break;
    //             case POS_ACC_OVER_LMT:
    //                 reportError(DRIVE_ERR_POS_ACC_OVER_LMT_J1 + (joint_num << ERROR_OFFSET_FOR_JOINT_NUM),
    //                             _("joint %d acceleration over limit!\n"),
    //                             ONE_BASED(joint_num));
    //                 break;
    //             case POS_CMD_OVER_LMT:
    //                 reportError(DRIVE_ERR_POS_CMD_OVER_LMT_J1 + (joint_num << ERROR_OFFSET_FOR_JOINT_NUM),
    //                             _("joint %d command position  over limit!\n"),
    //                             ONE_BASED(joint_num));
    //                 break;
    //             case CAN_OFF_LINE:
    //                 if (zucmotStatus->powered_on)
    //                 {
    //                     reportError(DRIVE_ERR_CAN_OFF_LINE_J1 + (joint_num << ERROR_OFFSET_FOR_JOINT_NUM), _("joint %d CAN off line!\n"), ONE_BASED(joint_num));
    //                 }
    //                 break;
    //             case UNDER_VOLTAGE:
    //                 reportError(
    //                     DRIVE_ERR_UNDER_VOLTAGE_J1 + (joint_num << ERROR_OFFSET_FOR_JOINT_NUM), _("joint %d servo under voltage!\n"), ONE_BASED(joint_num));
    //                 break;
    //             case OVER_VOLTAGE:
    //                 reportError(
    //                     DRIVE_ERR_OVER_VOLTAGE_J1 + (joint_num << ERROR_OFFSET_FOR_JOINT_NUM), _("joint %d servo over voltage!\n"), ONE_BASED(joint_num));
    //                 break;
    //             case FUSE_OPEN:
    //                 reportError(DRIVE_ERR_FUSE_OPEN_J1 + (joint_num << ERROR_OFFSET_FOR_JOINT_NUM), _("joint %d fuse open\n"), ONE_BASED(joint_num));
    //                 break;
    //             case DRIVER_OVERTEMP:
    //                 reportError(DRIVE_ERR_DRIVER_OVERTEMP_J1 + (joint_num << ERROR_OFFSET_FOR_JOINT_NUM),
    //                             _("joint %d servo over temperature!\n"),
    //                             ONE_BASED(joint_num));
    //                 break;
    //             case ENC_AUTOTURN_ERR:
    //                 reportError(DRIVE_ERR_ENC_AUTO_TURN_ERR_J1 + (joint_num << ERROR_OFFSET_FOR_JOINT_NUM),
    //                             _("joint %d encoder auto-turn error!\n"),
    //                             ONE_BASED(joint_num));
    //                 break;
    //             case OUT_LACK_PHASE:
    //                 reportError(
    //                     DRIVE_ERR_OUT_LACK_PHASE_J1 + (joint_num << ERROR_OFFSET_FOR_JOINT_NUM), _("joint %d out lack of phase!\n"), ONE_BASED(joint_num));
    //                 break;
    //             case POWER_ID_ERR:
    //                 reportError(DRIVE_ERR_POWER_ID_ERR_J1 + (joint_num << ERROR_OFFSET_FOR_JOINT_NUM),
    //                             _("parameter settings of joint %d do not match with power driver\n"),
    //                             ONE_BASED(joint_num));
    //                 break;
    //             case EEPROM_ERR:
    //                 reportError(DRIVE_ERR_EEPROM_ERR_J1 + (joint_num << ERROR_OFFSET_FOR_JOINT_NUM), _("joint %d eeprom error!\n"), ONE_BASED(joint_num));
    //                 break;
    //             case POWERON_FAILED:
    //                 if (zucmotStatus->powered_on)
    //                 {
    //                     reportError(
    //                         DRIVE_ERR_POWERON_FAILED_J1 + (joint_num << ERROR_OFFSET_FOR_JOINT_NUM), _("joint %d failed to power on!\n"), ONE_BASED(joint_num));
    //                 }

    //                 break;
    //             case MOTOR_I2T:
    //                 reportError(DRIVE_ERR_MOTOR_I2T_J1 + (joint_num << ERROR_OFFSET_FOR_JOINT_NUM), _("joint %d motor I2T!\n"), ONE_BASED(joint_num));
    //                 break;
    //             case BRAKE_ERR:
    //                 reportError(DRIVE_ERR_BRAKE_ERR_J1 + (joint_num << ERROR_OFFSET_FOR_JOINT_NUM), _("joint %d brake error!\n"), ONE_BASED(joint_num));
    //                 break;

    //             case ENC_INTERNAL_ERR:
    //                 reportError(DRIVE_ERR_ENC_INTERNAL_ERR_J1 + (joint_num << ERROR_OFFSET_FOR_JOINT_NUM),
    //                             _("joint %d encoder internal error!\n"),
    //                             ONE_BASED(joint_num));
    //                 break;

    //             case ENC_MAG_SIGNAL_ERR:
    //                 reportError(DRIVE_ERR_ENC_MAG_SIGNAL_ERR_J1 + (joint_num << ERROR_OFFSET_FOR_JOINT_NUM),
    //                             _("joint %d encoder magnetic signal error!\n"),
    //                             ONE_BASED(joint_num));
    //                 break;

    //             case POS_DEVIATION_ERR:

    //                 reportError(DRIVE_ERR_POS_DEVIATION_ERR_J1 + (joint_num << ERROR_OFFSET_FOR_JOINT_NUM),
    //                             _("joint %d encoder position deviation stop!\n"),
    //                             ONE_BASED(joint_num));
    //                 break;

    //             case POS_DEVIATION_WARNING:

    //                 reportWarning(DRIVE_ERR_POS_DEVIATION_WARNING_J1 + (joint_num << ERROR_OFFSET_FOR_JOINT_NUM),
    //                               _("joint %d encoder position deviation warnning!\n"),
    //                               ONE_BASED(joint_num));
    //                 break;

    //             case ENC_EEPROM_ERR:
    //                 reportError(
    //                     DRIVE_ERR_ENC_EEPROM_ERR_J1 + (joint_num << ERROR_OFFSET_FOR_JOINT_NUM), _("joint %d encoder e2prom error!\n"), ONE_BASED(joint_num));
    //                 break;

    //             case ENC_OVERTEMP:
    //                 // if (over_temp_is_new || !counter_5m)
    //                 {
    //                     // over_temp_is_new = 0;
    //                     reportWarning(DRIVE_ERR_ENC_OVERTEMP_J1 + (joint_num << ERROR_OFFSET_FOR_JOINT_NUM),
    //                                   _("joint %d encoder over temperature error!\n"),
    //                                   ONE_BASED(joint_num));
    //                 }
    //                 break;
    //             case UNMATCHED_SERVO_VERSION:
    //                 reportError(DRIVE_ERR_UNMATCHED_VERSION_J1 + (joint_num << ERROR_OFFSET_FOR_JOINT_NUM),
    //                             _("joint %d unmatched servo version\n"),
    //                             ONE_BASED(joint_num));
    //                 break;

    //             case DC_OVERCURRENT:
    //                 reportError(DRIVE_ERR_DC_OVERCURRENT_J1 + (joint_num << ERROR_OFFSET_FOR_JOINT_NUM), _("joint %d DC_OVERCURRENT\n"), ONE_BASED(joint_num));
    //                 break;

    //             case OVER_MAX_ACC:
    //                 reportError(DRIVE_ERR_OVER_MAX_ACC_J1 + (joint_num << ERROR_OFFSET_FOR_JOINT_NUM), _("joint %d OVER_MAX_ACC\n"), ONE_BASED(joint_num));
    //                 break;

    //             case ENC_BATTERY_ALM:
    //                 reportError(
    //                     DRIVE_ERR_ENC_BATTERY_ALM_J1 + (joint_num << ERROR_OFFSET_FOR_JOINT_NUM), _("joint %d ENC_BATTERY_ALM\n"), ONE_BASED(joint_num));
    //                 break;
    //             case ENC_BATTERY_ERR:
    //                 reportError(
    //                     DRIVE_ERR_ENC_BATTERY_ERR_J1 + (joint_num << ERROR_OFFSET_FOR_JOINT_NUM), _("joint %d ENC_BATTERY_ERR\n"), ONE_BASED(joint_num));
    //                 break;

    //             case ENC_DUAL_ERR:
    //                 reportError(DRIVE_ERR_ENC_DUAL_ERR_J1 + (joint_num << ERROR_OFFSET_FOR_JOINT_NUM), _("joint %d ENC_DUAL_ERR\n"), ONE_BASED(joint_num));
    //                 break;
    //             case ENC_Z_MISS:
    //                 reportError(DRIVE_ERR_ENC_Z_MISS_J1 + (joint_num << ERROR_OFFSET_FOR_JOINT_NUM), _("joint %d ENC_Z_MISS\n"), ONE_BASED(joint_num));
    //                 break;
    //             case IGBT_I2T:
    //                 reportError(DRIVE_ERR_IGBT_I2T_J1 + (joint_num << ERROR_OFFSET_FOR_JOINT_NUM), _("joint %d IGBT_I2T\n"), ONE_BASED(joint_num));
    //                 break;
    //             case OVER_POWER:
    //                 reportError(DRIVE_ERR_OVER_POWER_J1 + (joint_num << ERROR_OFFSET_FOR_JOINT_NUM), _("joint %d OVER_POWER\n"), ONE_BASED(joint_num));
    //                 break;
    //             case IN_LACK_PHASE:
    //                 reportError(DRIVE_ERR_IN_LACK_PHASE_J1 + (joint_num << ERROR_OFFSET_FOR_JOINT_NUM), _("joint %d IN_LACK_PHASE\n"), ONE_BASED(joint_num));
    //                 break;
    //             case CONNECT_ERR:
    //                 reportError(DRIVE_ERR_INER_CONNECT_ERR_J1 + (joint_num << ERROR_OFFSET_FOR_JOINT_NUM), _("joint %d CONNECT_ERR\n"), ONE_BASED(joint_num));
    //                 break;
    //             case FPGA_ERROR:
    //                 reportError(DRIVE_ERR_PARA_ERROR_J1 + (joint_num << ERROR_OFFSET_FOR_JOINT_NUM), _("joint %d FPGA_ERROR\n"), ONE_BASED(joint_num));
    //                 break;
    //             case PID_OVERFLOW:
    //                 reportError(DRIVE_ERR_PID_OVERFLOW_J1 + (joint_num << ERROR_OFFSET_FOR_JOINT_NUM), _("joint %d PID_OVERFLOW\n"), ONE_BASED(joint_num));
    //                 break;
    //             case RobotSerie_ERR:
    //                 reportError(DRIVE_ERR_RobotSerie_ERR_J1 + (joint_num << ERROR_OFFSET_FOR_JOINT_NUM), _("joint %d RobotSerie_ERR\n"), ONE_BASED(joint_num));
    //                 break;
    //             case MOTOR_OVERTEMP:
    //                 reportError(DRIVE_ERR_MOTOR_OVERTEMP_J1 + (joint_num << ERROR_OFFSET_FOR_JOINT_NUM), _("joint %d MOTOR_OVERTEMP\n"), ONE_BASED(joint_num));
    //                 break;
    //             case DBRAKE_OVER_LOAD:
    //                 reportError(DRIVE_ERR_DBRAKE_OVER_LOAD_J1 + (joint_num << ERROR_OFFSET_FOR_JOINT_NUM),
    //                             _("joint %d high frequency injection failed\n"),
    //                             ONE_BASED(joint_num));
    //                 break;

    //             default: {
    //                 int servoErrType = (servoStatus[joint_num].errorCode >> 8) & 0xff;
    //                 if (servoErrType == 0x60)
    //                 {
    //                     int finalErrorCode = 0x100000 + (joint_num << ERROR_OFFSET_FOR_JOINT_NUM) + (0xFFFF & servoStatus[joint_num].errorCode);
    //                     reportError(
    //                         finalErrorCode, _("joint %d encoder internal error, errcode is %d!\n"), ONE_BASED(joint_num), servoStatus[joint_num].errorCode);
    //                 }
    //                 else
    //                 {
    //                     reportError(DRIVE_ERR_UNKNOWN,
    //                                 _("joint %d unknown error, error_code is %x!#{\"fixed_key\":[\"%d\",\"0x%x\"]} \n"),
    //                                 ONE_BASED(joint_num),
    //                                 servoStatus[joint_num].errorCode,
    //                                 ONE_BASED(joint_num),
    //                                 servoStatus[joint_num].errorCode);
    //                 }
    //                 break;
    //             }
    //             }
    //         }

    //         /*reset sevo only when in fault state*/
    //         if (servoStatus[joint_num].errorCode != 0)  // 有伺服报错就重置和清除故障码
    //         {
    //             // servoReset();
    //             // if ((zucmot_hal_data->motors_servo_on) == 0)
    //             // servoClearCollision();
    //             rtapi_print("servo error or warning reset\n");
    //         }
    //     }
    //     // last_errorCode[joint_num] = servoStatus[joint_num].errorCode;

    //     if (zucmotDebug->enabling)
    //     {
    //         if (RUN_MODE_REAL == zucmotConfig->safetyCfg.motRunMode)
    //         {
    //             if (last_connectState[joint_num] && !servoStatus[joint_num].connectState)
    //             {
    //                 reportError(DRIVE_ERR_CONNECT_ERR_J1 + (joint_num << ERROR_OFFSET_FOR_JOINT_NUM), _("joint %d connection error!\n"), ONE_BASED(joint_num));
    //                 // rtapi_print(" ################################### [collision_rebound 888] ################################### \n");
    //                 motAbortMotion();
    //    //                 // tpManager_tpclear();
    //                 // zucmotStatus->last_momentum_movescale = 1.0;
    //                 // zucmotStatus->last_power_movescale = 1.0;
    //                 // zucmotStatus->momvelscale = 1.0;
    //                 // ZERO_ZUC_POSE(zucmotStatus->dtg);
    //                 // SET_MOTION_INPOS_FLAG(1);
    //                 zucmotDebug->enabling = 0;
    //                 zucmotDebug->dragging = 0;
    //                 // *(zucmot_hal_data->zuc_enable_cmd) = 0;
    //                 SET_MOTION_ERROR_FLAG(1);
    //                 servoEnable(0);
    //             }
    //         }
    //     }
    //     last_connectState[joint_num] = servoStatus[joint_num].connectState;
    // }

    // /*when an error is detected, and all joints are normal*/
    // if (last_errorCodeAll != 0 && !any_joint_in_fault() && !any_unhandled_error())
    // {
    //     rtapi_print("###last_errorCodeAll set to 0 from %d\n", last_errorCodeAll);
    //     last_errorCodeAll = 0;
    // }
    canmonitor_record_timer(26);

    if (zucmotConfig->cab_type > 1)
    {
        // scbStatus = SCBRefreshStatus();
        // long unifiedErrCode = 0;
        // if ((scbStatus.estop_signla_abnormal != last_scb_estop_signal_abnormal) && scbStatus.estop_signla_abnormal != 0)
        // {
        //     unifiedErrCode = 0x300000 + scbStatus.estop_signla_abnormal;
        //     reportError(unifiedErrCode, "scb error occured, error code is 0x%lx", unifiedErrCode);
        //     error_state = 1;
        // }
        // last_scb_estop_signal_abnormal = scbStatus.estop_signla_abnormal;
        // if ((scbStatus.errorCode != last_scb_errCode) && scbStatus.errorCode)
        // {
        //     unifiedErrCode = 0x300000 + scbStatus.errorCode;
        //     switch (unifiedErrCode)
        //     {
        //     case SCB_ERR_TCASE_ABNORMAL:             // Braking resister is over temperature
        //     case SCB_ERR_BRAKE_RESISTOR_OVER_TEMPE:  // Overtemperature of braking resistor
        //     {
        //         reportError(unifiedErrCode, "scb error occured, error code is 0x%lx", unifiedErrCode);
        //         *zucmot_hal_data->zuc_enable_cmd = 0;
        //         break;
        //     }
        //     case SCB_ERR_EMEGENCY_STOP:  // Emergency Stop
        //     {
        //         break;
        //     }
        //     case SCB_ERR_24V_VOLTAGE_ABNORMAL:  // 24V power supply abnormal
        //     {
        //         reportError(unifiedErrCode, "scb error occured, error code is 0x%lx", unifiedErrCode);
        //         if (GET_MOTION_COORD_FLAG() && *(zucmot_hal_data->program_stat_idle) == 0)
        //         {
        //        //             tpManager_tpabort();
        //             zucmotDebug->coordinating = 0;
        //         }
        //         break;
        //     }
        //     case SCB_ERR_DRAG_OVER_SPEED:  // tool center drag over speed
        //     {
        //         // char str[256];
        //         // snprintf(str, 256,"{\"fixed_key\":[\"%.3f\",\"%.3f\"]}", zucmotConfig->safetyCfg.dragTcpSpeedLimit, zucmotStatus->vel_tcp_linear);
        //         // reportError(SCB_ERR_DRAG_OVER_SPEED, "scb error occured, error code is 0x%lx  # %s ", unifiedErrCode, str);
        //         // if (GET_MOTION_DRAG_FLAG())
        //         // {
        //         //     zucmotDebug->dragging = 0;  // only for miniCobo
        //         // }
        //         break;
        //     }
        //     case SCB_ERR_BP_ESTOP_INCONSISTENT:   // handle emergency stop signal inconsistent
        //     case SCB_ERR_USR_ESTOP_INCONSISTENT:  // user emergency stop signal input (EI) inconsistent
        //     case SCB_ERR_ADDI_ESTOP_ABNORMAL:     // 附加急停信号输入异常
        //     {
        //         break;
        //     }
        //     case SCB_ERR_PROTECTIVE_STOP_INCONSISTENT:                      // protective stop signal input (SI) inconsistent
        //     case SCB_ERR_ADDI_PSTOP_ABNORMAL:                               // 附加保护性停止信号输入异常
        //     case SCB_ERR_ADDI_PSTOP_RESET_ABNORMAL:                         // 保护性停止复位信号输入异常
        //     case SCB_ERR_REDUCE_INPUT_ABNORMAL:                             // 衰减模式信号输入异常
        //     case SCB_ERR_THREE_POSITION_ENABLING_INPUT_ABNORMAL:            //三位置使能输入信号异常
        //     case SCB_ERR_CLOSED_COLLISION_DETECTION_SIGNAL_INPUT_ABNORMAL:  //关闭碰撞检测信号输入异常
        //     case SCB_ERR_COLLISION_DETECTION_LV1_SIGNAL_INPUT_ABNORMAL:     //碰撞灵敏度LV1信号输入异常
        //     case SCB_ERR_COLLISION_DETECTION_LV2_SIGNAL_INPUT_ABNORMAL:     //碰撞灵敏度LV2信号输入异常
        //     case SCB_ERR_COLLISION_DETECTION_LV3_SIGNAL_INPUT_ABNORMAL:     //碰撞灵敏度LV3信号输入异常
        //     case SCB_ERR_COLLISION_DETECTION_LV4_SIGNAL_INPUT_ABNORMAL:     //碰撞灵敏度LV4信号输入异常
        //     case SCB_ERR_COLLISION_DETECTION_LV5_SIGNAL_INPUT_ABNORMAL:     //碰撞灵敏度LV5信号输入异常
        //     case SCB_ERR_COLLISION_DETECTION_LV7_SIGNAL_INPUT_ABNORMAL:     //碰撞灵敏度LV7,碰撞灵敏度极低信号输入异常
        //     {
        //         reportError(unifiedErrCode, "scb error occured, error code is 0x%lx", unifiedErrCode);
        //         if (zucmotStatus->powered_on == 1)
        //         {
        //             error_state = 1;
        //         }
        //         break;
        //     }
        //     case SCB_ERR_SAFETY_SIGNAL_ABNORMAL_REFUSE_POWERON: {
        //         reportWarning(unifiedErrCode, "The safety digital signal anomaly has not been repaired SCB refuses to power on the robot ");
        //         *zucmot_hal_data->zuc_enable_cmd = 0;
        //         break;
        //     }
        //     case SCB_ERR_CAN_NORMAL_WAIT_TIMEOUT:
        //     case SCB_ERR_BODY_CAN_COMM_ABNORMAL: {
        //         int flag = 1;
        //         for (int i = 0; i < zucmotConfig->numJoints; i++)
        //         {
        //             if (is_servo_sim(i))
        //             {
        //                 flag = 0;
        //                 break;
        //             }
        //         }
        //         if (flag)
        //         {
        //             reportError(unifiedErrCode, "scb error occured, error code is 0x%lx", unifiedErrCode);
        //             *(zucmot_hal_data->zuc_enable_cmd) = 0;
        //         }
        //         // *(zucmot_hal_data->zuc_enable_cmd) = 0;
        //         break;
        //     }
        //     case SCB_ERR_PDU_TEMPERATURE_ABNORMAL:  // PDU temperature abnormal
        //     case SCB_ERR_220V_VOLTAGE_ABNORMAL:     // 220V power supply abnormal
        //     case SCB_ERR_220V_PWR_RALAY_ABNORMAL:   // main supply relay is abnormal
        //     case SCB_ERR_OUT_BODY_SUPPLY_ABNORMAL:  // supply voltage of output body abnormal
        //     case SCB_ERR_RELAY_ABNORMAL:            // PSCB relay abnormal
        //     case SCB_ERR_RELAY_ADHESION:            // relay adhesion
        //     case SCB_ERR_IPC_CURRENT_ABNORMAL:      // ipc current abnormal
        //     case SCB_ERR_FAN_CURRENT_ABNORMAL:      // fan current abnormal
        //     case SCB_ERR_BODY_SUPPLY_ABNORMAL_L1:   // output body supply current abnormal level 1 warning:25A
        //     case SCB_ERR_BODY_SUPPLY_ABNORMAL_L2:   // output body supply current abnormal level 2 warning:31.25A
        //     case SCB_ERR_BODY_SUPPLY_ABNORMAL_L3:   // output body supply current abnormal level 3 warning:37.25A
        //     case SCB_ERR_USR_IO_SUPPLY_ABNORMAL:    // supply current of user IO abnormal
        //     case SCB_ERR_ROBOT_POWER_EXCEPTION:     // 本体功率异常
        //     {
        //         reportWarning(unifiedErrCode, "scb error occured, error code is 0x%lx", unifiedErrCode);
        //         break;
        //     }
        //     case SCB_ERR_ROBOT_POWER_LIMIT: {
        //         reportWarning(unifiedErrCode, "scb error occured, hit robot power limit : 0x%lx", unifiedErrCode);
        //         break;
        //     }
        //     default: {
        //         reportWarning(unifiedErrCode, "scb error occurred");
        //         break;
        //     }
        //     }
        // }
        // last_scb_errCode = scbStatus.errorCode;
        // zucmotStatus->is_robot_power_limited = (last_scb_errCode == SCB_ERR_ROBOT_POWER_LIMIT) || zucmotStatus->is_robot_power_limited;
    }

    // for (int i = 0; i < zucmotConfig->numJoints; i++)
    // {
    //     //rtapi_print("[handle_servo_error] :servoStatus[%d].servoVersion  = %d \n",joint,servoStatus[joint].servoVersion);
    //     if (0 == servoStatus[i].servoVersion && queryServoVersionTimeOut[i])
    //     {
    //         queryServoVersionTimeOut[i]++;
    //     }
    //     if ((queryServoVersionTimeOut[i] >= TIMER_20S) && (zucmotConfig->safetyCfg.motRunMode == RUN_MODE_REAL))
    //     {
    //         rtapi_print("queryServoVersionTimeOut %d, joint is sim ? %d, %d\n", i, is_servo_sim(i), servoStatus[i].servoVersion);
    //         if (!is_servo_sim(i))
    //         {
    //             reportError(DRIVE_QUERY_VERSION_TIMEOUT, _("query servo version time out!\n"));
    //         }
    //         queryServoVersionTimeOut[i] = 0;
    //     }
    // }

    // for (joint_num = 0; joint_num < zucmotConfig->numJoints; joint_num++)
    // {
    //     if (servoStatus[joint_num].canError)
    //     {
    //         reportError(CAN_MASTER_SERVO_MODE_ERROR, "servo state machine error: joint %d!\n", ONE_BASED(joint_num));
    //         break;
    //     }
    // }

    /**
     * 暂时保留error_state后续安全信号异常改为SCB告知伺服执行Cat.1 Stop后在做更改
     * 
     * 目前的设计需要控制器规划减速完成以后在进行safety_signal_abnormal置位，因为此状态置位后指令规划会停止，指令使用反馈来覆盖
    */
    if (1 == error_state)
    {
        if (GET_MOTION_ENABLE_FLAG() && zucmotStatus->safety_signal_abnormal == 0)
        {
            // SET_MOTION_ERROR_FLAG(1);
            motAbortMotion();
            //            tpManager_tpabort();
            zucmotStatus->safety_signal_abnormal = 1;
        }

        if (zucmotStatus->current_vel <= 0.01 && GET_MOTION_ENABLE_FLAG())
        {
            zucmotDebug->enabling = 0;
            zucmotStatus->safety_signal_abnormal = 1;
        }

        if (zucmotStatus->powered_on == 1 && !zucmotStatus->emergency_stop && !GET_MOTION_ENABLE_FLAG())
        {
            zucmot_hal_data->zuc_enable_cmd = 0;
        }

        if (zucmotStatus->powered_on == 0)
        {
            error_state = 0;
            zucmotStatus->safety_signal_abnormal = 0;
        }
    }
}

static void handle_drag_req()
{
    // int axis_num = 0;
    int joint_num = 0;
    int servotorFlag = 0;
    // zucmot_joint_t *joint;
    // zucmot_axis_t *axis;
    static unsigned char last_freeKey_state;
    static unsigned char last_UIKey_state;
    static int warning_triggerred = 0;
    // for (joint_num = 0; joint_num < zucmotConfig->numJoints; joint_num++)
    // {
    //     if (servoStatus[joint_num].torOver)
    //     {
    //         servotorFlag = 1;
    //         break;
    //     }
    // }

    // tio_button    free  button
    if (zucmotStatus->freeButtonOpt == ON)
    {
        if (zucmotStatus->tio_status.UIKey_free && !last_freeKey_state)
        {  // rising edge
            if ((servotorFlag == 1) && !warning_triggerred)
            {
                reportError(CANNOT_DRAG_WHEN_SERVOTOROVERRUN, "can not enter drag mode when servo torque overrun");
                warning_triggerred = 1;
            }
            else if (zucmotStatus->sFuncDISettings.drag_limit != 0 && zucmotStatus->three_position_enable_limit == 1)
            {
                //如果配置文件中配置了三位置使能限制拖拽，并且三位置使能为触发状态禁止进入拖拽模式
                reportError(ERROR_THREE_POSITION_ENABLING_LIMIT, "Can't drag when the three positions enable limits");
            }
            else
            {
                // handle drag entr form tio key
                zucmotDebug->dragging = 1;
                zucmot_hal_data->drag_mode_req = 1;
                warning_triggerred = 0;
            }
        }
    }

    //FIXME:改为Standby状态下进行检测，而不是free
    if (zucmotStatus->freeButtonOpt == OFF && zucmotStatus->motion_state == ZUCMOT_MOTION_FREE)
    {
        if (zucmotStatus->tio_status.UIKey_free && !last_freeKey_state)
        {
            reportInfo(CANNOT_DRAG_WHEN_TURN_OFF_FREE_BUTTON, "can not enter drag mode when turn  off tio  free  button");
        }
    }

    // tio_button
    if ((!zucmotStatus->tio_status.UIKey_free && last_freeKey_state))
    {  // falling edge
        // handle drag exit form tio key
        zucmot_hal_data->drag_mode_req = 0;
        warning_triggerred = 0;
        zucmotDebug->dragging = 0;
    }

    // tio_button  free prog button
    if (zucmotStatus->secondfreeButtonOpt == ON)
    {
        if (zucmotStatus->tio_status.UIKey_pause_resume && !last_UIKey_state)
        {  // rising edge
            if ((servotorFlag == 1) && !warning_triggerred)
            {
                reportError(CANNOT_DRAG_WHEN_SERVOTOROVERRUN, "can not enter drag mode when servo torque overrun");
                warning_triggerred = 1;
            }
            else if (zucmotStatus->sFuncDISettings.drag_limit != 0 && zucmotStatus->three_position_enable_limit == 1)
            {
                //如果配置文件中配置了三位置使能限制拖拽，并且三位置使能为触发状态禁止进入拖拽模式
                reportError(ERROR_THREE_POSITION_ENABLING_LIMIT, "Can't drag when the three positions enable limits");
            }
            else
            {
                // handle drag entr form tio key
                zucmotDebug->dragging = 1;
                zucmot_hal_data->drag_mode_req = 1;
                warning_triggerred = 0;
            }
        }
    }

    if (zucmotStatus->tio_status.UIKey_pause_resume && !last_UIKey_state)
    {  // rising edge

        //FIXME:改为Standby状态下进行检测，而不是free
        if (zucmotStatus->secondfreeButtonOpt == OFF && zucmotStatus->motion_state == ZUCMOT_MOTION_FREE)
        {
            reportInfo(CANNOT_DRAG_WHEN_TURN_OFF_FREE_BUTTON, "can not enter drag mode when turn  off tio  free  button");
        }
        if (zucmotStatus->secondfreeButtonOpt || zucmotStatus->progButtonOpt)
        {  // 末端灯按钮没有被禁用, 按下按钮的同时可解除碰撞
            if (zucmotStatus->protective_stop)
            {
                zucmotStatus->protective_stop = 0;
                reportInfo(INFO_ROBOT_COLLISION_STOP_RESET, "robot exit from collision protective stop");
            }
        }
    }

    // tio_button free prog button
    if ((!zucmotStatus->tio_status.UIKey_pause_resume && last_UIKey_state) && zucmotStatus->secondfreeButtonOpt == ON)
    {  // falling edge
        // handle drag exit form tio key
        zucmot_hal_data->drag_mode_req = 0;
        warning_triggerred = 0;
        zucmotDebug->dragging = 0;
    }

    last_freeKey_state = zucmotStatus->tio_status.UIKey_free;
    last_UIKey_state = zucmotStatus->tio_status.UIKey_pause_resume;
}

static void handle_point_button_req()
{
    static unsigned char last_pointKey_state;
    if (last_pointKey_state && !zucmotStatus->tio_status.UIKey_point)
    {  // falling edge
        if (zucmotStatus->pointButtonOpt == OFF)
        {
            reportInfo(CANNOT_RECORD_POINT_WHEN_TURN_OFF_POINT_BUTTON, "can not record point when turn off the point button");
        }
    }
    last_pointKey_state = zucmotStatus->tio_status.UIKey_point;
}

static void handle_category_one_stop()
{
    /*如果没有配置急停程序暂停的话直接return*/
    if (!zucmotConfig->cat1StopEffect.program_pause)
    {
        return;
    }
    zucmotStatus->category_one_stop = zucmotStatus->safety_signal_abnormal || zucmotStatus->emergency_stop;
    /**
    * 如果在等待程序恢复执行阶段，因为error或者其他原因导致程序停止执行则退出等待状态
    */
    if (!zucmotDebug->coordinating || zucmot_hal_data->program_stat_idle)
    {
        if (zucmotStatus->category_one_stop_program_pause)
        {
            zucmotStatus->category_one_stop_program_pause = 0;
        }
        if (estop_motion_plan_flag)
        {
            estop_motion_plan_flag = 0;
        }
        if (zucmotStatus->estop_recovery)
        {
            zucmotStatus->estop_recovery = 0;
        }
    }
    //切换Cat.1 Stop暂停状态位，保证控制器切换disable状态时不会清除tp
    if (zucmotStatus->category_one_stop && !zucmotStatus->category_one_stop_program_pause &&
        (zucmot_hal_data->program_stat_running || zucmot_hal_data->program_stat_paused || zucmot_hal_data->stepping_stat_running) &&
        (zucmotStatus->tracking == 0 && zucmotStatus->convyr_type == 0 &&
         zucmotStatus->compliantType == 0))  // 急停按钮被按下, 传送带跟踪过程中拍下急停：停止程序； 力控中也停止程序
    {
        zucmotStatus->category_one_stop_program_pause = 1;  //set
    }

    /*暂时先这样写，主要是为了仅记录一次Cat.1 Stop 发生后的点位*/
    int last = estop_motion_plan_flag;
    if (1 == zucmotStatus->category_one_stop_program_pause && !GET_MOTION_ENABLE_FLAG())
    {
        estop_motion_plan_flag = 1;
    }
    /*此处记录的点位并非Cat.1 Stop发生那一刻的点位，是Cat.1 Stop发生后控制器切换至下使能状态时的点位*/
    if (last == 0 && estop_motion_plan_flag == 1)
    {
        if (!zucmotStatus->estop_recovery)
        {
            // TC_STRUCT* temp_tc = tcqItem(&zucmotDebug->coord_tp.queue, 0);
            // TC_STRUCT* temp_nexttc = tcqItem(&zucmotDebug->coord_tp.queue, 1);
            // if (temp_tc != NULL)
            //            if (tpManager_GetMotId(0) > 0)
            {
                // zucmotStatus->estop_progress = temp_tc->estop_progress;
                // temp_tc->progress = temp_tc->estop_progress;
                //                zucmotStatus->estop_progress = tpManager_GetMotEStopProgress(0);
                //                tpManager_SetMotProgress(0, zucmotStatus->estop_progress);
                //                tpManager_tppause();
                // zucmotDebug->coord_tp.pausing = 1;
                // Process_cur_tc(temp_tc);
                // if (temp_nexttc && temp_nexttc->progress > Q_FUZZ)
                // {
                //     temp_nexttc->progress = temp_nexttc->estop_progress;
                //     Process_cur_tc(temp_nexttc);
                //     zucmotStatus->estop_nexttc_progress = temp_nexttc->estop_progress;
                // }
                //                tpManager_updateTcDataForEStop();
                //                if (tpManager_GetMotProgress(1) > Q_FUZZ)
                {
                    //                    zucmotStatus->estop_nexttc_progress = tpManager_GetMotProgress(1);
                    //                    tpManager_SetMotProgress(1, zucmotStatus->estop_nexttc_progress);
                    // Process_cur_tc(temp_nexttc);
                }
                //                // @shixiaoli 此处可能需要增加对应接口--->"movsTimeSacle = 0.0" finished in tpManager_updateTcDataForEStop()
                //process movs related parameters
                //                // zucmotDebug->coord_tp.movsTimeSacle = 0.0;finished in tpManager_updateTcDataForEStop()
                //                // zucmotStatus->reducel2_elb = 0;//been moved to tp,finished in tpManager_updateTcDataForEStop()
                //                // zucmotStatus->reducel2_tcp = 0;//been moved to tp,finished in tpManager_updateTcDataForEStop()
                //                // zucmotStatus->elbow_tcp_movs_del = 0;//been moved to tp,finished in tpManager_updateTcDataForEStop()

                //momentum related parameters
                //                // zucmotDebug->coord_tp.desired_velocity = 0.0;//finished in tpManager_updateTcDataForEStop()
                //                // zucmotDebug->coord_tp.current_velocity = 0.0;//finished in tpManager_updateTcDataForEStop()
                //恢复对轨迹复现急停复位后继续运行程序的支持
                // if (temp_tc->motion_type == TC_MOVS)
                // {
                //     tpAbort(&zucmotDebug->coord_tp);
                //     tpHandleAbort(&zucmotDebug->coord_tp, temp_tc, temp_nexttc);
                // }
            }

            // for (int i = 0; i < zucmotConfig->numJoints; i++)
            // {
            //     zucmotStatus->estop_joint_pos[i] = zucmotDebug->coord_tp.last_jpos_desired[i];
            //     zucmotStatus->estop_tp_goal_joint_pos[i] = zucmotDebug->coord_tp.goalPos_joint[i];
            //     zucmotDebug->coord_tp.currentPos_joint[i] = zucmotDebug->coord_tp.last_jpos_desired[i];
            // }
            // zucmotStatus->estop_cart_pos = zucmotDebug->coord_tp.last_cpos_desired;
            // zucmotStatus->estop_tp_goal_pos = zucmotDebug->coord_tp.goalPos;
            // zucmotDebug->coord_tp.currentPos = zucmotDebug->coord_tp.last_cpos_desired;
            //            // tpManager_tpRevertInterpolation();
            //            tpManager_tpGetCurJointPos(zucmotStatus->estop_joint_pos);
            //            tpManager_tpGetGoalJointPos(zucmotStatus->estop_tp_goal_joint_pos);
            //            tpManager_tpGetPose(&zucmotStatus->estop_cart_pos);
            //            tpManager_tpGetGoalPose(&zucmotStatus->estop_tp_goal_pos);
            //            tpManager_InitRecoverTc();
        }
    }

    /*Cat.1 Stop后程序保持暂停状态，此时点击程序继续执行，从这里开始reset*/
    if (!zucmotStatus->category_one_stop && zucmotStatus->category_one_stop_program_pause &&
        (zucmot_hal_data->program_stat_running || zucmot_hal_data->stepping_stat_running) && GET_MOTION_ENABLE_FLAG())
    {
        zucmotStatus->category_one_stop_program_pause = 0;  // reset
        estop_motion_plan_flag = 0;
        zucmotStatus->estop_recovery = 1;
        // add estop_tc
        // TC_STRUCT* tc;
        // tc = tcqItem(&zucmotDebug->coord_tp.queue, 0);
        // TC_STRUCT* nexttc = tcqItem(&zucmotDebug->coord_tp.queue, 1);
        // //        if (tpManager_GetMotId(0) > 0)
        // {
        //     double joint_fb_pos[ZUCMOT_MAX_JOINTS] = {0.0};
        //     for (int joint_num = 0; joint_num < zucmotConfig->numJoints; joint_num++) { joint_fb_pos[joint_num] = joints[joint_num].pos_fb; }
        //     //            int ret = tpManager_AddRecoverTc(joint_fb_pos, &zucmotStatus->carte_pos_fb, &zucmotStatus->estop_cart_pos, zucmotStatus->estop_joint_pos);

        //     //tpSetPos中把currentPos也改掉了，这是不应该的，后面要改回去
        //     // tpSetPos(&zucmotDebug->coord_tp, &zucmotStatus->estop_tp_goal_pos, zucmotStatus->estop_tp_goal_joint_pos);
        //     //            tpManager_tpSetPos_JPos(&zucmotStatus->estop_tp_goal_pos, zucmotStatus->estop_tp_goal_joint_pos);
        //     // for (int i = 0; i < zucmotConfig->numJoints; i++) { zucmotDebug->coord_tp.currentPos_joint[i] = zucmotStatus->estop_joint_pos[i]; }
        //     // zucmotDebug->coord_tp.currentPos = zucmotStatus->estop_cart_pos;
        //     // zucmotStatus->carte_pos_cmd = zucmotDebug->coord_tp.currentPos;
        //     // zucmotStatus->last_carte_pos_cmd = zucmotDebug->coord_tp.currentPos;
        //     // zucmotStatus->last_last_carte_pos_cmd = zucmotDebug->coord_tp.currentPos;
        //     // tc->progress = zucmotStatus->estop_progress;
        //     //            tpManager_tpSetCurJpos(zucmotStatus->estop_joint_pos);  //把上面tpSetPos中把currentPos修改的地方改回去
        //     //            tpManager_tpSetPose(zucmotStatus->estop_cart_pos);
        //     // zucmotStatus->carte_pos_cmd = zucmotStatus->estop_cart_pos;
        //     // zucmotStatus->last_carte_pos_cmd = zucmotStatus->estop_cart_pos;
        //     // zucmotStatus->last_last_carte_pos_cmd = zucmotStatus->estop_cart_pos;
        //     //            tpManager_SetMotProgress(0, zucmotStatus->estop_progress);
        //     //            tpManager_SetMotProgress(1, zucmotStatus->estop_nexttc_progress);
        // }
        // else { zucmotStatus->estop_recovery = 0; }
    }
}

void check_singularity(double* q_in, const int last_cycle_toppra)
{
    // static double q[ZUCMOT_MAX_JOINTS];
    // static PmHomogeneous hom_crd;  // 6th coordinate to base coordinate
    // static float last_cosq2 = 0.0;
    // static float last_cosq4 = 0.0;
    // static float decParam[2] = {3000, 720};
    // static float singDectectCoef[3] = {100, 60, 60};
    // static float last_shoulderDist = 0.0;
    // static int last_singularity_check_flag = 0;
    // if (singularity_check_flag || last_cycle_toppra)
    // {
    //     int i = 0;
    //     float shoulderDist = 0, shoulderVel = 0;
    //     for (i = 0; i < zucmotConfig->numJoints; i++) { q[i] = q_in[i] * PM_PI / 180.0; }
    //     kineCalcEndRefPose(q_in, &hom_crd);
    //     if (zucmotConfig->robotModel / 10000 == 1)
    //     {
    //         shoulderDist = sqrt(hom_crd.tran.x * hom_crd.tran.x + hom_crd.tran.y * hom_crd.tran.y - zucmotStatus->dhParam.d[1] * zucmotStatus->dhParam.d[1]);
    //     }
    //     else
    //     {
    //         shoulderDist = sqrt(hom_crd.tran.x * hom_crd.tran.x + hom_crd.tran.y * hom_crd.tran.y - zucmotStatus->dhParam.d[3] * zucmotStatus->dhParam.d[3]);
    //     }

    //    //     if (!last_singularity_check_flag || last_cycle_toppra || tpManager_tpSych_singular_dist() == 1)
    //     {
    //         last_cosq2 = fabs(cos(q[2]));
    //         last_cosq4 = fabs(cos(q[4]));
    //         joints[2].vel_cmd = 0.0;
    //         joints[4].vel_cmd = 0.0;
    //         last_shoulderDist = shoulderDist;
    //    //         tpManager_tpResetSych_singular_dist();
    //     }
    //     shoulderVel = (shoulderDist - last_shoulderDist) / zucmotConfig->trajCycleTime;
    //     if (zucmotConfig->robotModel / 10000 == 1)
    //     {
    //         decParam[0] = 1000;  // pos acc in dec stage
    //         decParam[1] = 270;   // rot acc in dec stage
    //         singDectectCoef[0] = shoulderVel / 50.0;
    //         singDectectCoef[1] = joints[2].vel_cmd / 40.0;
    //         singDectectCoef[2] = joints[4].vel_cmd / 40.0;
    //     }
    //     else
    //     {
    //         decParam[0] = 3000;  // pos acc in dec stage
    //         decParam[1] = 720;   // rot acc in dec stage
    //         singDectectCoef[0] = shoulderVel / 100.0;
    //         singDectectCoef[1] = joints[2].vel_cmd / 60.0;
    //         singDectectCoef[2] = joints[4].vel_cmd / 60.0;
    //     }
    //     int bsingularity = 0;
    //     if (fabs(cos(q[2])) > cos(0.1745 * singDectectCoef[1]) && (fabs(cos(q[2])) - last_cosq2) > 1e-3)
    //     {
    //         rtapi_print("joint 3 sigularity, q[2] = %f, singDectectCoef[1] = %f\n", q[2], singDectectCoef[1]);
    //         bsingularity = 1;
    //     }
    //     else if (fabs(cos(q[4])) > cos(0.1745 * singDectectCoef[2]) && (fabs(cos(q[4])) - last_cosq4) > 1e-3)
    //     {
    //         rtapi_print("joint 5 sigularity, q[4] = %f, singDectectCoef[2] = %f\n", q[4], singDectectCoef[2]);
    //         bsingularity = 1;
    //     }
    //     else if (shoulderDist < fabs(20.0 * singDectectCoef[0]) && shoulderVel < 0)
    //     {
    //         rtapi_print("joint 1 sigularity, shoulderDist = %f, lastshoulderDist = %f, coef = %f, shoulderVel = %f, bound = %f\n",
    //                     shoulderDist,
    //                     last_shoulderDist,
    //                     singDectectCoef[0],
    //                     shoulderVel,
    //                     fabs(20.0 * singDectectCoef[0]));
    //         bsingularity = 1;
    //     }
    //     // if( (fabs(cos(q[2])) > cos(0.1745*singDectectCoef[1]) && (fabs(cos(q[2])) - last_cosq2) > 0) ||
    //     // 	(fabs(cos(q[4])) > cos(0.1745*singDectectCoef[2]) && (fabs(cos(q[4])) - last_cosq4) > 0) ||
    //     // 	(shoulderDist < fabs(20.0*singDectectCoef[0]) && shoulderVel < 0)){
    //     if (bsingularity)
    //     {
    //         if (GET_MOTION_TELEOP_FLAG())
    //         {
    //             for (i = 0; i < CART_DIM; i++)
    //             {
    //                 axes[i].teleop_tp.enable = 0;
    //                 axes[i].teleop_tp.max_acc = (i < 3) ? decParam[0] : decParam[1];
    //             }
    //         }
    //         else if (GET_MOTION_TELEOP_TOOL_FLAG())
    //         {
    //             for (i = 0; i < CART_DIM; i++)
    //             {
    //                 axes[i].teleop_tool_tp.enable = 0;
    //                 axes[i].teleop_tool_tp.max_acc = (i < 3) ? decParam[0] : decParam[1];
    //             }
    //         }
    //         else if (GET_MOTION_ADMITTANCE_FLAG())
    //         {
    //             zucmotDebug->admitting = 0;
    //         }
    //         else if (GET_MOTION_COORD_FLAG())
    //         {
    //    //             tpManager_tpabort();
    //             if ((zucmotStatus->convyr_type == 1 && zucmotConfig->lctsynch) || zucmotStatus->convyr_type == 2)
    //             {
    //                 zucmotStatus->pre_singular = 1;
    //                 if (get_robot_joint_collision_sensitivity() == 0x0F)
    //                 {
    //                     set_robot_joint_collision_sensitivity(zucmotConfig->safetyCfg.clsn_sensitivity);
    //                 }
    //             }
    //         }
    //         if ((zucmot_hal_data->motion_error_code) != STOP_ON_SIGULARITY || !GET_MOTION_ERROR_FLAG())
    //         {
    //             reportError(STOP_ON_SIGULARITY, _("Protective stop since it's nearby singularity pistion____9."));
    //         }
    //         SET_MOTION_ERROR_FLAG(1);
    //     }
    //     last_cosq2 = fabs(cos(q[2]));
    //     last_cosq4 = fabs(cos(q[4]));
    //     last_shoulderDist = shoulderDist;
    // }

    // last_singularity_check_flag = singularity_check_flag;
}

static void lowPassFilterForDrag(double cutFrq, double* input, double* output, int i)
{
    double k = 0;
    k = 1.0 / 2.0 / M_PI / cutFrq;

    double k1 = 1.0 / (1.0 + 2 * k / 0.008);
    double k2 = -(1.0 - 2 * k / 0.008) / (1.0 + 2 * k / 0.008);

    output[i] = k1 * (input[i] + last_drag_x[i]) + k2 * last_drag_y[i];

    //update
    last_drag_y[i] = output[i];
    last_drag_x[i] = input[i];
}

static void get_curr_cmds()
{
    int joint_num;
    zucmot_joint_t* joint;
    double currCmd[ZUCMOT_MAX_JOINTS] = {0.0};
    double currGrvCmd[ZUCMOT_MAX_JOINTS] = {0.0};
    double jntPosDeg[ZUCMOT_MAX_JOINTS] = {0.0};  // joint position in deg
    double jntPos[ZUCMOT_MAX_JOINTS] = {0.0};     // joint position in rad
    // double jntVel[ZUCMOT_MAX_JOINTS] = {0.0};		// joint speed in rad/s
    int i = 0;
    // int includeGrvflag = -2;
    // static int flag1_count = 0;
    // static int flag2_count = 0;
    // static int flag0_count = 0;

    /*take the gravity component into account*/
    for (joint_num = 0; joint_num < zucmotConfig->numJoints; ++joint_num)
    {
        // static double lastJntPos[ZUCMOT_MAX_JOINTS] = {0.0};
        jntPosDeg[joint_num] = joints[joint_num].pos_cmd;
        jntPos[joint_num] = joints[joint_num].pos_cmd * PM_PI / 180.0;
        // jntVel[joint_num] = (jntPos[joint_num]-lastJntPos[joint_num])/zucmotConfig->trajCycleTime;
        // lastJntPos[joint_num] = jntPos[joint_num];
    }

    /*calculate the dynamics*/
    double torq[ZUCMOT_MAX_JOINTS] = {0.0};
    double torf[ZUCMOT_MAX_JOINTS] = {0.0};
    double torg[ZUCMOT_MAX_JOINTS] = {0.0};
    double q[ZUCMOT_MAX_JOINTS] = {0.0};
    double qd[ZUCMOT_MAX_JOINTS] = {0.0};
    double qdd[ZUCMOT_MAX_JOINTS] = {0.0};
    static double lastq[ZUCMOT_MAX_JOINTS] = {0.0};
    static double lastqd[ZUCMOT_MAX_JOINTS] = {0.0};
    // static double lastqdd[ZUCMOT_MAX_JOINTS] = {0.0};
    static int collision_rebound_time = 0;
    int rest_state = 1;
    double aborting_jntPos[ZUCMOT_MAX_JOINTS] = {0};
    double aborting_jntVel[ZUCMOT_MAX_JOINTS] = {0};
    // includeGrvflag = zucmotStatus->includeGrvFlag;
    // rtapi_print("DH = %f %f %f %f %f %f\n", dhParameters[0],dhParameters[1],dhParameters[2],dhParameters[3],dhParameters[4],dhParameters[5]);
    // rtapi_print("Angle = %f %f %f\n", installAngle[0], installAngle[1], installAngle[2]);
    // rtapi_print("toolParameters = %f %f %f %f\n", toolParameters[0], toolParameters[1], toolParameters[2], toolParameters[3]);
    // rtapi_print("q = %f %f %f %f\n", q[0], q[1], q[2], q[3]);
    // rtapi_print("qd = %f %f %f %f\n", qd[0], qd[1], qd[2], qd[3]);
    // rtapi_print("qdd = %f %f %f %f\n", qdd[0], qdd[1], qdd[2], qdd[3]);

    for (joint_num = 0; joint_num < zucmotConfig->numJoints; joint_num++)
    {
        q[joint_num] = jntPos[joint_num];
        qd[joint_num] = (q[joint_num] - lastq[joint_num]) / zucmotConfig->trajCycleTime;
        qdd[joint_num] = (qd[joint_num] - lastqd[joint_num]) / zucmotConfig->trajCycleTime;
    }

    static double last_x = 0.0;
    static double last_y = 0.0;
    static double last_z = 0.0;
    double dx = zucmotStatus->carte_pos_fb.tran.x - last_x;
    double dy = zucmotStatus->carte_pos_fb.tran.y - last_y;
    double dz = zucmotStatus->carte_pos_fb.tran.z - last_z;
    last_x = zucmotStatus->carte_pos_cmd.tran.x;
    last_y = zucmotStatus->carte_pos_cmd.tran.y;
    last_z = zucmotStatus->carte_pos_cmd.tran.z;
    double linear_vel = sqrt(dx * dx + dy * dy + dz * dz) / zucmotConfig->trajCycleTime;

    //    // if (tpManager_tpCollision_rebound() == 1 || tpManager_tpCollision_rebound() == 2) {		// 等待减速阶段
    // 	// 期望规划快，实际以正常加速度运行，避免超调过大，增强这一阶段的伺服调节能力
    // 	double highest_acc = 720.0 / 180 * PM_PI;
    // 	if (linear_vel < 300) {
    // 		highest_acc = highest_acc * linear_vel / axes[0].vel_limit;
    // 		if (highest_acc < 10.0 / 180/0 * PM_PI) {
    // 			highest_acc = 10.0;
    // 		}
    // 	}
    // 	for (int i = 0; i < zucmotConfig->numJoints; i++) {
    // 		if (fabs(qdd[i]) > highest_acc) {
    // 			qdd[i] = qdd[i] > 0 ? highest_acc : -highest_acc;
    // 		}
    // 	}
    // }

    for (int i = 0; i < zucmotConfig->numJoints; i++)
    {
        if (fabs(qd[i]) < 0.001)
        {
            qd[i] = 0.0;
        }
        if (fabs(qdd[i]) < 0.001)
        {
            qdd[i] = 0.0;
        }
    }

    // 未上使能状态，指令值跟反馈值抖动
    if (!zucmotDebug->enabling)
    {
        for (int i = 0; i < zucmotConfig->numJoints; i++)
        {
            if (fabs(qd[i]) < 0.005 && fabs(qdd[i]) < 0.6)
            {
                qd[i] = 0.0;
                qdd[i] = 0.0;
            }
        }
    }

    // for avoiding large torque when command catch feedback
    static int rebound_flag = 0;
    static int rebound_end_count = 0;
    //    if (rebound_flag != 5 && tpManager_tpCollision_rebound() == 5)
    {
        for (joint_num = 0; joint_num < zucmotConfig->numJoints; joint_num++)
        {
            qd[joint_num] = 0;
            qdd[joint_num] = 0;
        }
        // 提高机器人在反弹时的轨迹跟踪能力
        if (zucmotConfig->safetyCfg.clsn_sensitivity < 5 && zucmotConfig->safetyCfg.clsn_sensitivity != 0)
        {
            // set_robot_joint_collision_sensitivity(5);
        }
    }
    //    if (rebound_flag == 5 && tpManager_tpCollision_rebound() != 5)
    {
        rebound_end_count = 1;
        // for (int i = 0; i < zucmotConfig->numJoints; i++) { set_robot_joint_avoid_pos_mutation(i, 0); }
    }
    //    // if (tpManager_tpCollision_rebound() == 5)
    // {
    //     if (zucmotDebug->coord_tp.current_velocity < Q_FUZZ)
    //     {
    //         rtapi_print("Now Collision rebound stop!\n");
    //         zucmotDebug->coord_tp.collision_rebound = 0;
    //         rebound_end_count = 1;
    //     }
    // }
    if (rebound_end_count > 0)
    {
        rebound_end_count++;
        if (rebound_end_count > 10)
        {
            // 恢复
            rtapi_print("recover zucmotConfig->safetyCfg.clsn_sensitivity %d\n", zucmotConfig->safetyCfg.clsn_sensitivity);
            // set_robot_joint_collision_sensitivity(zucmotConfig->safetyCfg.clsn_sensitivity);
            rebound_end_count = 0;
        }
    }

    // for realize free mode after rebound end
    // static int last_collision_rebound = 0;
    static int free_count = 0;
    // static int wait_rebound_over = 0;
    if (zucmotStatus->protective_stop)
    {
        free_count++;
    }
    else
    {
        free_count = 0;
        // wait_rebound_over = 0;
    }

    // 非使能情况下，无速度加速度，解决上电瞬间位置跳变问题
    if (!zucmotDebug->enabling)
    {
        for (int i = 0; i < zucmotConfig->numJoints; i++)
        {
            qd[i] = 0;
            qdd[i] = 0;
        }
    }
    //    // rtapi_print("motion state : %d, zucmotStatus->protective_stop : %d, tpManager_tpCollision_rebound() : %d, free_count : %d\n",
    //    //  zucmotStatus->motion_state, zucmotStatus->protective_stop, tpManager_tpCollision_rebound(), free_count);
    // 暂停时不跟踪反馈位置，@FIXME 后面应对其进行处理
    // if (zucmotStatus->protective_stop)
    // {
    //     rtapi_print("zucmotStatus->motion_state = %d;\n", zucmotStatus->motion_state);
    //     rtapi_print("zucmotStatus->paused = %d;\n", zucmotStatus->paused);
    // }
    // if (access("/home/jakauser/cc.txt", 0) == 0)
    // {
    //     rtapi_print("zucmotStatus->motion_state = %d;\n", zucmotStatus->motion_state);
    //     rtapi_print("zucmotStatus->paused = %d;\n", zucmotStatus->paused);
    // }
    //     if (zucmotStatus->protective_stop && !zucmotStatus->paused)
    //     {
    //    //         // if (last_collision_rebound == 5 && tpManager_tpCollision_rebound() == 0) {
    //         // 	wait_rebound_over = 1;
    //         // }
    //         // if (wait_rebound_over) {			// 为避免碰撞反弹规划结束，但实际机器人仍未到位，给与50个周期等待时间, 无法知道之前是否反弹，放弃
    //         // 	wait_rebound_over++;
    //         // 	if (wait_rebound_over > 50) {
    //         // 		wait_rebound_over = 50;
    //         // 	}
    //         // }
    //         // 避免反弹时间过长，60个周期后，停止指令 @FIXME 需通知伺服
    //    //         int collision_rebound = tpManager_tpCollision_rebound();
    //         if ((collision_rebound == 0 && free_count == 50) || free_count > 60)
    //         {
    //             if (collision_rebound != 0)
    //             {
    //                 rtapi_print("Abort Rebound!\n");
    //                 // tpAbort(&zucmotDebug->coord_tp);
    //    //                 tpManager_tpabort();
    //                 motAbortMotion();
    //             }
    //             // 非反弹、非自动模式才允许跟踪位置
    //             // FREE 模式时，是否处于规划运动可由Inpos标志位判断，据此，在在Inpos才可跟踪位置，避免JOG减速中误跟踪
    //             // FIXME: 碰撞后切换到了ErrorStop状态，此状态下允许指令跟反馈（人为推动关节）

    //             if (collision_rebound == 0 && zucmotStatus->motion_state == ZUCMOT_MOTION_FREE && GET_MOTION_INPOS_FLAG())
    //             {
    //                 free_count = 0;
    //                 for (joint_num = 0; joint_num < zucmotConfig->numJoints; joint_num++)
    //                 {
    //                     joints[joint_num].pos_cmd = joints[joint_num].pos_fb;
    //                     joints[joint_num].free_tp.curr_pos = joints[joint_num].pos_fb;
    //                     q[joint_num] = joints[joint_num].pos_fb * PM_PI / 180.0;
    //                     qd[joint_num] = 0;
    //                     qdd[joint_num] = 0;
    //                 }
    //             }
    //         }
    //         // rtapi_print("motion state : %d\n", zucmotStatus->motion_state);
    //     }

    //     if (estop_motion_plan_flag && GET_MOTION_ENABLE_FLAG())
    //     {
    //         // printf("############ Hold ######## \n");
    //         for (joint_num = 0; joint_num < zucmotConfig->numJoints; joint_num++)
    //         {
    //             // joints[joint_num].pos_cmd = joints[joint_num].pos_fb;
    //             // joints[joint_num].free_tp.curr_pos = joints[joint_num].pos_fb;
    //             // q[joint_num] = joints[joint_num].pos_fb * PM_PI / 180.0;
    //             joints[joint_num].pos_cmd = last_posbf_before_enable[joint_num];
    //             joints[joint_num].free_tp.curr_pos = last_posbf_before_enable[joint_num];
    //             q[joint_num] = last_posbf_before_enable[joint_num] * PM_PI / 180.0;
    //             qd[joint_num] = 0;
    //             qdd[joint_num] = 0;
    //         }
    //     }

    //    //     // last_collision_rebound = tpManager_tpCollision_rebound();

    //     static UINT8 lastGravityOnly = 0;
    //     static UINT8 gravityDelay = 0;  //退出拖拽时前馈仅重力直到各轴恢复位置模式
    //     if (GET_MOTION_DRAG_FLAG() || GET_MOTION_ADMITTANCE_FLAG() || GET_MOTION_HOME_FLAG())
    //     {
    //         lastGravityOnly = 1;
    //         getJointGf(q, qd, qdd, torg, torf);
    //         if (zucmotStatus->torque_control_settings.torque_control_enable)
    //         {
    //             TorqueControlVal var;
    //             int is_got_var = 0;
    //             int sem_id = sem_get(TORQUE_CONTROL_SEM_KEY);
    //             if (sem_p(sem_id) == 0)
    //             {
    //                 if (!TorqueControlQueIsEmpty(&zucmotTorqueControlStruct->torque_que))
    //                 {
    //                     TorqueControlDequeue(&zucmotTorqueControlStruct->torque_que, &var);
    //                     is_got_var = 1;
    //                 }
    //                 sem_v(sem_id);
    //             }
    //             if (is_got_var)
    //             {
    //                 zucmotStatus->torque_control_settings.torque_last_count = zucmotStatus->torque_control_settings.torque_last_period;
    //                 for (i = 0; i < ZUCMOT_MAX_JOINTS; i++) { zucmotStatus->torque_control_settings.current_torque_val[i] = var.torque[i]; }
    //             }
    //             else
    //             {
    //                 zucmotStatus->torque_control_settings.torque_last_count--;
    //                 if (zucmotStatus->torque_control_settings.torque_last_count > 0)
    //                 {
    //                     // 保持上一周期的值
    //                     ;
    //                 }
    //                 else
    //                 {
    //                     zucmotStatus->torque_control_settings.torque_last_count = -1;
    //                     // 未收到，则发重力
    //                     for (i = 0; i < zucmotConfig->numJoints; i++) { zucmotStatus->torque_control_settings.current_torque_val[i] = torg[i]; }
    //                 }
    //             }
    //             for (i = 0; i < zucmotConfig->numJoints; i++) { currGrvCmd[i] = zucmotStatus->torque_control_settings.current_torque_val[i]; }
    //             // rtapi_print("Torque Control: %f, %f, %f, %f, %f, %f, %f\n",
    //             //             zucmotStatus->torque_control_settings.current_torque_val[0],
    //             //             zucmotStatus->torque_control_settings.current_torque_val[1],
    //             //             zucmotStatus->torque_control_settings.current_torque_val[2],
    //             //             zucmotStatus->torque_control_settings.current_torque_val[3],
    //             //             zucmotStatus->torque_control_settings.current_torque_val[4],
    //             //             zucmotStatus->torque_control_settings.current_torque_val[5],
    //             //             zucmotStatus->torque_control_settings.current_torque_val[6]);
    //         }
    //         else if (zucmotStatus->drag_control_setiings.drag_control_enable)
    //         {
    //             double tfc[ZUCMOT_MAX_JOINTS] = {};
    //             double tfv[ZUCMOT_MAX_JOINTS] = {};

    //             // rtapi_print("qd = %lf\n", qd[5]);

    //             for (int i = 0; i < zucmotConfig->numJoints; i++)
    //             {
    //                 if (fabs(qd[i]) < 2.0 / 180.0 * M_PI)
    //                 {
    //                     qd[i] = 0;
    //                 }
    //             }

    //             getJointF(q, qd, qdd, tfc, tfv);

    //             double f1[ZUCMOT_MAX_JOINTS] = {};
    //             double f2[ZUCMOT_MAX_JOINTS] = {};
    //             for (int i = 0; i < zucmotConfig->numJoints; i++)
    //             {
    //                 f1[i] = tfc[i] * zucmotStatus->drag_control_setiings.drag_control_fc_compensate[i] / 100.0 +
    //                         tfv[i] * zucmotStatus->drag_control_setiings.drag_control_fv_compensate[i] / 100.0;

    //                 if (zucmotStatus->drag_control_setiings.drag_control_filter_factor[i] != 0)
    //                 {
    //                     lowPassFilterForDrag(zucmotStatus->drag_control_setiings.drag_control_filter_factor[i], f1, f2, i);
    //                     currGrvCmd[i] = torg[i] + f2[i];
    //                 }
    //                 else
    //                 {
    //                     currGrvCmd[i] = torg[i] + f1[i];
    //                 }
    //             }
    //         }
    //         else
    //         {
    //             for (i = 0; i < zucmotConfig->numJoints; i++) { currGrvCmd[i] = torg[i]; }
    //         }
    //     }
    //     else
    //     {
    //         if (lastGravityOnly)
    //         {
    //             gravityDelay = 1;
    //         }
    //         lastGravityOnly = 0;
    //         if (gravityDelay == 1)
    //         {
    //             int allInCspMode = 1;
    //             getJointGf(q, qd, qdd, torg, torf);
    //             for (i = 0; i < zucmotConfig->numJoints; i++)
    //             {
    //                 currGrvCmd[i] = torg[i];
    //                 allInCspMode = allInCspMode && (servoStatus[i].actualOpMode == SERVO_OP_MODE_CSP);
    //             }
    //             if (allInCspMode)
    //             {
    //                 gravityDelay = 0;
    //             }
    //         }
    //         else
    //         {
    //             int servostate = 0;
    //             for (int i = 0; i < zucmotConfig->numJoints; i++)
    //             {
    //                 if (servoStatus[i].actualOpMode == SERVO_OP_MODE_HOME)
    //                 {
    //                     servostate = 1;
    //                 }
    //             }
    //             if (servostate == 1)
    //             {
    //                 getJointGf(q, qd, qdd, torg, torf);
    //                 for (i = 0; i < zucmotConfig->numJoints; i++) { currGrvCmd[i] = torg[i]; }
    //             }
    //             else
    //             {
    //                 getTa(q, qd, qdd, torq);
    //                 for (i = 0; i < zucmotConfig->numJoints; i++) { currGrvCmd[i] = torq[i]; }
    //             }
    //         }
    // #ifdef WRITE_JOINT_FILE
    //         getTa(q, qd, qdd, toppra_tor);
    // #endif
    //     }

    //    // if (tpManager_tpCollision_rebound() == 1 || tpManager_tpCollision_rebound() == 2)
    // {
    //     // rtapi_print("currGrvCmd: %lf %lf %lf %lf %lf %lf\n", currGrvCmd[0], currGrvCmd[1], currGrvCmd[2], currGrvCmd[3], currGrvCmd[4], currGrvCmd[5]);
    //     // rtapi_print("currGrvCmd: %lf %lf %lf %lf %lf %lf\n",
    //     //             zucmotStatus->ext_tor[0],
    //     //             zucmotStatus->ext_tor[1],
    //     //             zucmotStatus->ext_tor[2],
    //     //             zucmotStatus->ext_tor[3],
    //     //             zucmotStatus->ext_tor[4],
    //     //             zucmotStatus->ext_tor[5]);
    //     if (access("/home/jakauser/add.txt", 0) == 0)
    //     {
    //         for (int i = 0; i < zucmotConfig->numJoints; i++) { currGrvCmd[i] += zucmotStatus->ext_tor[i]; }
    //     }
    // }

    for (joint_num = 0; joint_num < zucmotConfig->numJoints; joint_num++)
    {
        lastq[joint_num] = q[joint_num];
        lastqd[joint_num] = qd[joint_num];
    }

    static int last_cycle_toppra = 0;
    //    if (tpManager_GetCurTrajType() == TOPP_PLANNER)
    // {
    //     last_cycle_toppra = 1;
    // }
    // else
    // {
    //     // check_singularity(jntPosDeg, last_cycle_toppra);  //TODO:当使用toppra的时候不再进行奇异预判，不过在task层逆解得时候需要主要处理奇异
    //     last_cycle_toppra = 0;
    // }

    //
    static double last_curr[ZUCMOT_MAX_JOINTS] = {0};
    //    if (rebound_flag != 5 && tpManager_tpCollision_rebound() == 5)
    // {
    //     for (int i = 0; i < zucmotConfig->numJoints; i++) { currGrvCmd[i] = last_curr[i]; }
    // }
    // for (int i = 0; i < zucmotConfig->numJoints; i++) { last_curr[i] = currGrvCmd[i]; }
    //    rebound_flag = tpManager_tpCollision_rebound();

    /* take the friction into account */
    for (joint_num = 0; joint_num < zucmotConfig->numJoints; ++joint_num)
    {
        zucmot_motor_t* motor = &motors[joint_num];
        double convert_ratio = 8192.0 / (motor->rated_trans_ratio * motor->rated_torq);
        zucmotStatus->servo_status.jointMonitorData[joint_num].jointTorqueCmd = currGrvCmd[joint_num];
        currCmd[joint_num] = currGrvCmd[joint_num];

        /* TO DO: compensate friction only in drag mode*/

        /* take the gear ratio and motor const into account*/
        currGrvCmd[joint_num] = currGrvCmd[joint_num] * convert_ratio;
        currCmd[joint_num] = currCmd[joint_num] * convert_ratio;

        /* reverse the 1rt and 3rd current command*/
        // if(joint_num == 0 || joint_num == 2 || joint_num == 4 || joint_num == 5){
        // 	currGrvCmd[joint_num] = -currGrvCmd[joint_num];
        // 	currCmd[joint_num] = -currCmd[joint_num];
        // }
    }

    /* copy joint position feedback to local array */
    for (joint_num = 0; joint_num < zucmotConfig->numJoints; joint_num++)
    {
        joint = &joints[joint_num];
        joint->motor_curr_cmd = (int)(currCmd[joint_num]);
        cur_cmd[joint_num] = joint->motor_curr_cmd;
        grv_cur_cmd[joint_num] = (int)(currGrvCmd[joint_num]);
        //rtapi_print(" joint  %d torque = %d \n ",joint_num,cur_cmd[joint_num]);
    }
    // sync_robot_torque(cur_cmd, grv_cur_cmd);

    // static int print_cnt = 0;
    // ++print_cnt;
    // if(print_cnt >= 100) {
    // 	rtapi_print("zg1= %f, zg2= %f, zg3= %f\n", zg1, zg2, zg3);
    // 	rtapi_print("current position: %f, %f, %f, %f, %f, %f\n", jntPos[0], jntPos[1], jntPos[2], jntPos[3], jntPos[4], jntPos[5]);
    // 	rtapi_print("motor current command: %f, %f, %f, %f, %f, %f\n", currCmd[0], currCmd[1], currCmd[2], currCmd[3], currCmd[4], currCmd[5]);
    // 	print_cnt = 0;
    // }

    // time_inst =  rtapi_get_time() - time_start;
    // time_max = time_inst > time_max ? time_inst : time_max;
    // if(time_cnt == 0) {
    //     time_ave = time_inst;
    // } else {
    //     time_ave = time_ave*(1.0*time_cnt/(time_cnt + 1)) + time_inst*1.0/(time_cnt + 1);
    // }
    // time_cnt++;

    // rtapi_print("get_torque_cmd time inst == %d\n\n", time_inst);
    // rtapi_print("get_torque_cmd time ave == %f\n\n", time_ave);
    // rtapi_print("get_torque_cmd time max == %d\n\n", time_max);
}

static void output_to_hal(void)
{
    int joint_num, axis_num;
    zucmot_joint_t* joint;
    zucmot_axis_t* axis;
    joint_hal_t* joint_data;
    axis_hal_t* axis_data;

    // static int old_motion_index=0, old_hal_index=0;
    // static int old_enable_state = 0;
    static int can_pos_cmd[ZUCMOT_MAX_JOINTS];
    unsigned char scb_bp_lamp = ZUC_BP_RGB_OFF;
    // SCBStatus scbStatus;

    /* output machine info to HAL for scoping, etc */
    (zucmot_hal_data->motion_enabled) = GET_MOTION_ENABLE_FLAG();
    (zucmot_hal_data->in_position) = GET_MOTION_INPOS_FLAG();
    (zucmot_hal_data->coord_mode) = GET_MOTION_COORD_FLAG();
    (zucmot_hal_data->teleop_mode) = GET_MOTION_TELEOP_FLAG();
    (zucmot_hal_data->teleop_tool_mode) = GET_MOTION_TELEOP_TOOL_FLAG();
    (zucmot_hal_data->coord_error) = GET_MOTION_ERROR_FLAG() || zucmotStatus->protective_stop;
    (zucmot_hal_data->on_soft_limit) = zucmotStatus->on_soft_limit;
    (zucmot_hal_data->next_to_limit) = zucmotStatus->next_to_limit;
    (zucmot_hal_data->emergency_stop) = zucmotStatus->emergency_stop;
    (zucmot_hal_data->in_stop_mode) = (zucmotStatus->percentage_mode_level == 3);
    (zucmot_hal_data->in_home_position) = zucmotStatus->in_home_position;

    (zucmot_hal_data->program_line) = zucmotStatus->id;
    (zucmot_hal_data->motion_type) = zucmotStatus->motionType;
    (zucmot_hal_data->distance_to_go) = zucmotStatus->distance_to_go;
    if (GET_MOTION_COORD_FLAG())
    {
        (zucmot_hal_data->current_vel) = zucmotStatus->current_vel;
        (zucmot_hal_data->requested_vel) = zucmotStatus->requested_vel;
    }
    else if (GET_MOTION_TELEOP_FLAG() || GET_MOTION_TELEOP_TOOL_FLAG())
    {
        int i;
        double v2 = 0.0;
        // for (i = 0; i < ZUCMOT_MAX_AXIS; i++)
        // {
        //     if (axes[i].teleop_tp.active)
        //         v2 += axes[i].teleop_vel_cmd * axes[i].teleop_vel_cmd;
        //     else if (axes[i].teleop_tool_tp.active)
        //         v2 += axes[i].teleop_tool_vel_cmd * axes[i].teleop_tool_vel_cmd;
        // }
        if (v2 > 0.0)
            zucmotStatus->current_vel = (zucmot_hal_data->current_vel) = sqrt(v2);
        else
            zucmotStatus->current_vel = (zucmot_hal_data->current_vel) = 0.0;
        (zucmot_hal_data->requested_vel) = 0.0;
    }
    else
    {
        int i;
        double v2 = 0.0;
        // for (i = 0; i < zucmotConfig->numJoints; i++)
        //     if (GET_JOINT_ACTIVE_FLAG(&(joints[i])) && joints[i].free_tp.active)
        //         v2 += joints[i].vel_cmd * joints[i].vel_cmd;
        if (v2 > 0.0)
            zucmotStatus->current_vel = (zucmot_hal_data->current_vel) = sqrt(v2);
        else
            zucmotStatus->current_vel = (zucmot_hal_data->current_vel) = 0.0;
        (zucmot_hal_data->requested_vel) = 0.0;
    }

    /* output joint info to HAL for scoping, etc */
    for (joint_num = 0; joint_num < zucmotConfig->numJoints; joint_num++)
    {
        joint = &joints[joint_num];
        joint->motor_pos_cmd = joint->pos_cmd + joint->backlash_filt;
        joint_data = &(zucmot_hal_data->joint[joint_num]);
        (joint_data->motor_pos_cmd) = joint->motor_pos_cmd;
        (joint_data->motor_curr_cmd) = joint->motor_curr_cmd;  // directly the calculated motor current command
        (joint_data->joint_pos_cmd) = joint->pos_cmd;
        // can_pos_cmd[joint_num] =
        //     (int)(ceil)((joint->pos_cmd - zucmotStatus->dhParam.joint_homeoff[joint_num]) * DEG2ABSCNT);  // 底层会进行补偿joint_offset, 此处无需再进行补偿
        (joint_data->joint_pos_fb) = joint->pos_fb;
        (joint_data->amp_enable) = GET_JOINT_ENABLE_FLAG(joint);
        (joint_data->index_enable) = joint->index_enable;
        (joint_data->homing) = GET_JOINT_HOMING_FLAG(joint);
        (joint_data->coarse_pos_cmd) = joint->coarse_pos;

        (joint_data->joint_vel_cmd) = joint->vel_cmd;
        (joint_data->backlash_corr) = joint->backlash_corr;
        (joint_data->backlash_filt) = joint->backlash_filt;
        (joint_data->backlash_vel) = joint->backlash_vel;
        (joint_data->f_error) = joint->ferror;
        (joint_data->f_error_lim) = joint->ferror_limit;
        // (joint_data->free_pos_cmd) = joint->free_tp.pos_cmd;
        // (joint_data->free_vel_lim) = joint->free_tp.max_vel;
        // (joint_data->free_tp_enable) = joint->free_tp.enable;
        (joint_data->kb_jjog_active) = joint->kb_jjog_active;
        (joint_data->wheel_jjog_active) = joint->wheel_jjog_active;
        (joint_data->active) = GET_JOINT_ACTIVE_FLAG(joint);
        (joint_data->in_position) = GET_JOINT_INPOS_FLAG(joint);
        (joint_data->error) = GET_JOINT_ERROR_FLAG(joint);
        (joint_data->phl) = GET_JOINT_PHL_FLAG(joint);
        (joint_data->nhl) = GET_JOINT_NHL_FLAG(joint);
        (joint_data->homed) = GET_JOINT_HOMED_FLAG(joint);
        (joint_data->f_errored) = GET_JOINT_FERROR_FLAG(joint);
        (joint_data->faulted) = GET_JOINT_FAULT_FLAG(joint);
        (joint_data->home_state) = joint->home_state;
        // send position command to CAN master
    }
    // sync_robot_position(can_pos_cmd, 8000);

    // TIO status lights
    if (zucmotStatus->emergency_stop || zucmotStatus->percentage_mode_level == 3)
    {
        tio_set_lamp(TIO_LAMP_RED);
        scb_bp_lamp = ZUC_BP_RGB_RED;
    }
    else if (!GET_MOTION_ENABLE_FLAG())
    {
        tio_set_lamp(TIO_LAMP_BLUE);
        scb_bp_lamp = ZUC_BP_RGB_BLUE;
    }
    else if (GET_MOTION_DRAG_FLAG())
    {
        tio_set_lamp(TIO_LAMP_YELLOW);
        scb_bp_lamp = ZUC_BP_RGB_YELLOW;
    }
    else if (GET_MOTION_COORD_FLAG() && zucmotStatus->paused)
    {
        tio_set_lamp(TIO_LAMP_YELLOW_TWINKLE);
        scb_bp_lamp = ZUC_BP_RGB_YELLOW;
    }
    else if (GET_MOTION_ERROR_FLAG() || zucmotStatus->protective_stop || zucmotStatus->on_soft_limit)
    {
        tio_set_lamp(TIO_LAMP_RED);
        scb_bp_lamp = ZUC_BP_RGB_RED;
    }
    else if (zucmotPNDevStruct->state.led_state)
    {
        tio_set_lamp(TIO_LAMP_GREEN);
        scb_bp_lamp = zucmotPNDevStruct->state.led_state == 1 ? ZUC_BP_RGB_GREEN : ZUC_BP_RGB_OFF;
    }
    //暂时弃用SCB端的拖拽速度限制功能，由控制器来实现
    // else if (zucmotConfig->cab_type > 1 && (SCB_ERR_DRAG_OVER_SPEED == 0x300000 + scbStatus.errorCode))
    // {
    //     tio_set_lamp(LAMP_RED);
    //     scb_bp_lamp = ZUC_BP_RGB_RED;
    // }

    // 仅作profinet认证使用
    // else if (zucmotPNDevStruct->state.led_state >= 0)
    // {
    //     tio_set_lamp(LAMP_GREEN);
    //     scb_bp_lamp = zucmotPNDevStruct->state.led_state == 1 ? ZUC_BP_RGB_GREEN : ZUC_BP_RGB_OFF;
    // }
    else
    {
        tio_set_lamp(TIO_LAMP_GREEN);
        scb_bp_lamp = ZUC_BP_RGB_GREEN;
    }

    if (zucmotConfig->cab_type > 1)
    {
        if (rtdev::RtDevMgr::instance().get_scb()->cab()->is_poweroff_ipc())
        {
            scb_bp_lamp = ZUC_BP_RGB_OFF;
        }

        set_bp_logo_color(scb_bp_lamp);
    }

    for (axis_num = 0; axis_num < ZUCMOT_MAX_AXIS; axis_num++)
    {
        axis = &axes[axis_num];
        axis_data = &(zucmot_hal_data->axis[axis_num]);

        (axis_data->teleop_vel_cmd) = axis->teleop_vel_cmd;
        // (axis_data->teleop_pos_cmd) = axis->teleop_tp.pos_cmd;
        // (axis_data->teleop_vel_lim) = axis->teleop_tp.max_vel;
        // (axis_data->teleop_tp_enable) = axis->teleop_tp.enable;

        (axis_data->teleop_tool_vel_cmd) = axis->teleop_tool_vel_cmd;
        // (axis_data->teleop_tool_pos_cmd) = axis->teleop_tool_tp.pos_cmd;
        // (axis_data->teleop_tool_vel_lim) = axis->teleop_tool_tp.max_vel;
        // (axis_data->teleop_tool_tp_enable) = axis->teleop_tool_tp.enable;

        (axis_data->kb_ajog_active) = axis->kb_ajog_active;
        (axis_data->wheel_ajog_active) = axis->wheel_ajog_active;
    }
    (zucmot_hal_data->axis[0].pos_cmd) = zucmotStatus->carte_pos_cmd.tran.x;
    (zucmot_hal_data->axis[1].pos_cmd) = zucmotStatus->carte_pos_cmd.tran.y;
    (zucmot_hal_data->axis[2].pos_cmd) = zucmotStatus->carte_pos_cmd.tran.z;
    (zucmot_hal_data->axis[3].pos_cmd) = zucmotStatus->carte_pos_cmd.a;
    (zucmot_hal_data->axis[4].pos_cmd) = zucmotStatus->carte_pos_cmd.b;
    (zucmot_hal_data->axis[5].pos_cmd) = zucmotStatus->carte_pos_cmd.c;
    (zucmot_hal_data->axis[6].pos_cmd) = zucmotStatus->carte_pos_cmd.u;
    (zucmot_hal_data->axis[7].pos_cmd) = zucmotStatus->carte_pos_cmd.v;
    (zucmot_hal_data->axis[8].pos_cmd) = zucmotStatus->carte_pos_cmd.w;
}

static void update_servo_enable_delay(void)
{
    static int last_servo_state = 0;
    static int servo_on_delay_cnt = 0;
    if (last_servo_state && !GET_MOTION_ENABLE_FLAG())
    {
        zucmotDebug->re_enable_delay = 1;
        servo_on_delay_cnt = 0;
    }
    if (zucmotDebug->re_enable_delay)
    {
        ++servo_on_delay_cnt;
        if (servo_on_delay_cnt >= 125)
        {
            zucmotDebug->re_enable_delay = 0;
            servo_on_delay_cnt = 0;
        }
    }
    last_servo_state = GET_MOTION_ENABLE_FLAG();
}

/**
 * @date    2024/03/07  1.7.1_27
 * @brief   calculate tcp angular velocity & acceleration with zucmotStatus data
 * @param   tool_A  array of last & now Cartesian orientation RX
 * @param   tool_B  array of last & now Cartesian orientation RY
 * @param   tool_C  array of last & now Cartesian orientation RZ
 * @param   angular_vel_tcp output of angular velocity
 * @param   angular_acc_tcp output of angular acceleration
 * @param   period  period of controller
 */
static int status_angular_velocity_acceleration_calculation(const double* tool_A,
                                                            const double* tool_B,
                                                            const double* tool_C,
                                                            double* angular_vel_tcp,
                                                            double* angular_acc_tcp,
                                                            double period)
{
    PmRpy last_pose_rpy, now_pose_rpy;
    static PmRpy rpy_before_last;
    PmQuaternion last_pose_quat, now_pose_quat, quat_before_last_pose, last_pose_quat_inv, quat_before_last_inv, delta_quat, last_delta_quat,
        last_delta_quat_inv, accel_quat;

    last_pose_rpy.y = tool_A[0] * PM_PI / 180.0;
    last_pose_rpy.p = tool_B[0] * PM_PI / 180.0;
    last_pose_rpy.r = tool_C[0] * PM_PI / 180.0;
    now_pose_rpy.y = tool_A[1] * PM_PI / 180.0;
    now_pose_rpy.p = tool_B[1] * PM_PI / 180.0;
    now_pose_rpy.r = tool_C[1] * PM_PI / 180.0;

    pmRpyQuatConvert(&last_pose_rpy, &last_pose_quat);
    pmRpyQuatConvert(&now_pose_rpy, &now_pose_quat);
    pmRpyQuatConvert(&rpy_before_last, &quat_before_last_pose);

    pmQuatInv(&last_pose_quat, &last_pose_quat_inv);
    pmQuatInv(&quat_before_last_pose, &quat_before_last_inv);
    pmQuatQuatMult(&now_pose_quat, &last_pose_quat_inv, &delta_quat);

    pmQuatQuatMult(&last_pose_quat, &quat_before_last_inv, &last_delta_quat);
    pmQuatInv(&last_delta_quat, &last_delta_quat_inv);
    pmQuatQuatMult(&delta_quat, &last_delta_quat_inv, &accel_quat);

    delta_quat.s = fabs(delta_quat.s) > 1.0 ? (delta_quat.s > 0 ? 1.0 : -1.0) : delta_quat.s;
    *angular_vel_tcp = fabs(2 * acos(delta_quat.s) * 180.0 / PM_PI) / period;

    accel_quat.s = fabs(accel_quat.s) > 1.0 ? (accel_quat.s > 0 ? 1.0 : -1.0) : accel_quat.s;
    *angular_acc_tcp = fabs(2 * acos(accel_quat.s) * 180.0 / PM_PI) / period / period;

    rpy_before_last.y = tool_A[0] * PM_PI / 180.0;
    rpy_before_last.p = tool_B[0] * PM_PI / 180.0;
    rpy_before_last.r = tool_C[0] * PM_PI / 180.0;
    return 0;
}

#define servoErrCodeUpdate(idx)                                                           \
    (((zucmot_hal_data->error_triggered) == true && (servoStatus[idx].errorCode != 0)) || \
     ((zucmot_hal_data->error_triggered) == false && (servoStatus[idx].errorCode == 0)))
#define collisionStateUpdate(idx)                                                              \
    (((zucmot_hal_data->error_triggered) == true && (servoStatus[idx].collisionState != 0)) || \
     ((zucmot_hal_data->error_triggered) == false && (servoStatus[idx].collisionState == 0)))
static void update_status(void)
{
    static int m = 0;
    // static int last_ioaio_conn = 0;
    static double tool_positionX[2] = {0};
    static double tool_positionY[2] = {0};
    static double tool_positionZ[2] = {0};
    static double tool_positionA[2] = {0};
    static double tool_positionB[2] = {0};
    static double tool_positionC[2] = {0};
    static double vel[CART_DIM] = {0};
    static double tool_offset[ZUCMOT_MAX_AXIS] = {0};
    static double user_offset[ZUCMOT_MAX_AXIS] = {0};
    static double tcp_pos[ZUCMOT_MAX_AXIS] = {0};
    ZucPose cartPoseTemp;
    int oldId, temp_var;
    int joint_num, axis_num, dio, aio, i;
    // double vel_x = 0, vel_y = 0, vel_z = 0, vel_a = 0, vel_b = 0, vel_c = 0;
    double vel_tcp = 0.0;
    double acc_tcp = 0.0;
    double angular_vel_tcp = 0.0;
    double angular_acc_tcp = 0.0;
    zucmot_joint_t* joint;
    zucmot_joint_status_t* joint_status;
    zucmot_axis_t* axis;
    zucmot_axis_status_t* axis_status;
    zucmot_motor_t* motor;

    bool servo_move_flag = GET_MOTION_SERVOMOVEOP_FLAG();
    bool admittance_flag = GET_MOTION_ADMITTANCE_FLAG();
    bool drag_flag = GET_MOTION_DRAG_FLAG();

    unsigned short motion_mode = ((servo_move_flag << 2) + (admittance_flag << 1) + drag_flag) & (255);
    zucmotStatus->motion_mode = motion_mode;
    /* set phyiscal IO */
    set_physic_IO_status();
    safety::get_safety_IO_status();
    safety::set_safety_IO_status();
    // zucmotStatus->commState = getTioCommState();
    ZERO_ZUC_POSE(cartPoseTemp);
    /*update the servo status*/
    if (RUN_MODE_REAL == zucmotConfig->safetyCfg.motRunMode || RUN_MODE_REAL_NO_CAN_CHECK == zucmotConfig->safetyCfg.motRunMode)
    {
        // zucmotStatus->servo_status.serial_num = get_robot_serial_num();
        // zucmotStatus->servo_status.servo_version = servoStatus[0].servoVersion;
        zucmotStatus->servo_status.clsn_sensitivity = zucmotConfig->safetyCfg.clsn_sensitivity;
    }
    zucmotStatus->servo_status.clsn_sensitivity = zucmotConfig->safetyCfg.clsn_sensitivity;
    // zucmotStatus->servo_status.first_poweron_date = servoStatus[0].firstPoweronDate;
    // zucmotExtIOStruct->MbSlaveShmStruct.zuc_cab_type = zucmotConfig->cab_type;

    for (joint_num = 0; joint_num < zucmotConfig->numJoints; joint_num++)
    {
        motor = &motors[joint_num];
        // float tempVal = servoStatus[joint_num].insAveragePower / 10.0;
        // zucmotStatus->servo_status.jointMonitorData[joint_num].insAveragePower = tempVal;
        // tempVal = (servoStatus[joint_num].actualTorque / 8192.0) * motor->rated_curr;
        // zucmotStatus->servo_status.jointMonitorData[joint_num].instCurrent = tempVal;
        // zucmotStatus->servo_status.jointMonitorData[joint_num].instVoltage = servoStatus[joint_num].instVoltage;
        // zucmotStatus->servo_status.jointMonitorData[joint_num].instTemperature = servoStatus[joint_num].instTemperature;
        // tempVal = servoStatus[joint_num].currentFluctuation / 10.0;
        // zucmotStatus->servo_status.jointMonitorData[joint_num].currentFluctuation = tempVal;
        // tempVal = servoStatus[joint_num].sumRunningCircles / 10.0;
        // zucmotStatus->servo_status.jointMonitorData[joint_num].sumRunningCircles = tempVal;
        // zucmotStatus->servo_status.jointMonitorData[joint_num].sumRunningTime = servoStatus[joint_num].sumRunningTime;
        // tempVal = servoStatus[joint_num].curRunningCircles / 10.0;
        // zucmotStatus->servo_status.jointMonitorData[joint_num].curRunningCircles = tempVal;
        // zucmotStatus->servo_status.jointMonitorData[joint_num].curRunningTime = servoStatus[joint_num].curRunningTime;
        // zucmotStatus->servo_status.motorSupplier[joint_num] = servoStatus[joint_num].motorSupplier;
        // zucmotStatus->servo_status.jointMonitorData[joint_num].positionDiff = servoStatus[joint_num].positionDiff / 8192;
        // zucmotStatus->servo_status.jointMonitorData[joint_num].dualEncoderDiff = servoStatus[joint_num].dualEncoderDiff / 8192 * 10.0;
        // zucmotStatus->servo_status.jointMonitorData[joint_num].jointDcVol = servoStatus[joint_num].jointDcVol / 8192 * 100;
        // zucmotStatus->servo_status.jointMonitorData[joint_num].joint_tor_sensor = (int16_t)servoStatus[joint_num].reserved / 8192.0 * 100.0;

        zucmotStatus->servo_status.jointMonitorData[joint_num].reserved = 0;
        // zucmotStatus->servo_status.jointMonitorData[joint_num].jointTorque =
        // (servoStatus[joint_num].actualTorque / 8192.0) * motors[joint_num].rated_trans_ratio * motors[joint_num].rated_torq;

        // zucmotStatus->servo_status.errorCode[joint_num] = servoStatus[joint_num].errorCode;
        // zucmotStatus->servo_status.collisionState[joint_num] = servoStatus[joint_num].collisionState;
        // zucmotStatus->servo_status.errorState[joint_num] = servoStatus[joint_num].errorState;
        // zucmotStatus->servo_status.enableState[joint_num] = servoStatus[joint_num].enableState;
    }

    /**TCP infomations */
    tool_positionX[1] = zucmotStatus->carte_pos_cmd.tran.x;
    tool_positionY[1] = zucmotStatus->carte_pos_cmd.tran.y;
    tool_positionZ[1] = zucmotStatus->carte_pos_cmd.tran.z;
    tool_positionA[1] = zucmotStatus->carte_pos_cmd.a;
    tool_positionB[1] = zucmotStatus->carte_pos_cmd.b;
    tool_positionC[1] = zucmotStatus->carte_pos_cmd.c;

    if (m == 0)
    {
        tool_positionX[0] = zucmotStatus->carte_pos_cmd.tran.x;
        tool_positionY[0] = zucmotStatus->carte_pos_cmd.tran.y;
        tool_positionZ[0] = zucmotStatus->carte_pos_cmd.tran.z;
        tool_positionA[0] = zucmotStatus->carte_pos_cmd.a;
        tool_positionB[0] = zucmotStatus->carte_pos_cmd.b;
        tool_positionC[0] = zucmotStatus->carte_pos_cmd.c;
        m++;
    }
    else
    {
        vel[0] = (tool_positionX[1] - tool_positionX[0]) / zucmotConfig->trajCycleTime;
        vel[1] = (tool_positionY[1] - tool_positionY[0]) / zucmotConfig->trajCycleTime;
        vel[2] = (tool_positionZ[1] - tool_positionZ[0]) / zucmotConfig->trajCycleTime;
        vel[3] = (tool_positionA[1] - tool_positionA[0]) / zucmotConfig->trajCycleTime;
        vel[4] = (tool_positionB[1] - tool_positionB[0]) / zucmotConfig->trajCycleTime;
        vel[5] = (tool_positionC[1] - tool_positionC[0]) / zucmotConfig->trajCycleTime;
        vel_tcp = sqrt(vel[0] * vel[0] + vel[1] * vel[1] + vel[2] * vel[2]);
        acc_tcp = (zucmotStatus->vel_tcp_linear - vel_tcp) / zucmotConfig->trajCycleTime;
        status_angular_velocity_acceleration_calculation(
            tool_positionA, tool_positionB, tool_positionC, &angular_vel_tcp, &angular_acc_tcp, zucmotConfig->trajCycleTime);
        tool_positionX[0] = tool_positionX[1];
        tool_positionY[0] = tool_positionY[1];
        tool_positionZ[0] = tool_positionZ[1];
        tool_positionA[0] = tool_positionA[1];
        tool_positionB[0] = tool_positionB[1];
        tool_positionC[0] = tool_positionC[1];
    }
    memcpy(zucmotStatus->vel_tcp, vel, sizeof(double) * CART_DIM);
    zucmotStatus->vel_tcp_linear = vel_tcp;
    zucmotStatus->vel_tcp_angular = angular_vel_tcp;
    zucmotStatus->acc_tcp_linear = acc_tcp;
    zucmotStatus->acc_tcp_angular = angular_acc_tcp;

    cartPoseTemp = zucmotConfig->tool_offset;
    memcpy(tool_offset, (const void*)&cartPoseTemp, sizeof(ZucPose));
    memcpy(zucmotStatus->tool_offset, (const void*)&cartPoseTemp, sizeof(ZucPose));

    cartPoseTemp = zucmotConfig->user_offset;
    memcpy(user_offset, (const void*)&cartPoseTemp, sizeof(ZucPose));
    memcpy(zucmotStatus->user_offset, (const void*)&cartPoseTemp, sizeof(ZucPose));

    cartPoseTemp = zucmotStatus->carte_pos_cmd;
    memcpy(tcp_pos, (const void*)&cartPoseTemp, sizeof(ZucPose));
    memcpy(zucmotStatus->tcp_pos, (const void*)&cartPoseTemp, sizeof(ZucPose));

    /*更新io数据*/
    // update_pn_status();
    // update_eip_status();
    // update_mb_status();

    /*update the cabinet status*/
    //FIXME: xxf 将其移动到cab自身的update中,类似robotproxy的那种
    auto rob = rtdev::RtDevMgr::instance().get_scb()->rob_power();
    zucmotStatus->cab_status.robotAveragePower = rob->get_power(0);
    zucmotStatus->cab_status.robotAverageCurrent = rob->get_current(0);
    zucmotStatus->cab_status.robotVoltage = rob->get_voltage(0);

    auto cab = rtdev::RtDevMgr::instance().get_scb()->cab();
    zucmotStatus->cab_status.cabTemperature = cab->get_temperature();
    zucmotStatus->cab_status.scbMajorVersion = cab->get_sw_ver();
    zucmotStatus->cab_status.scbMinorVersion = cab->get_sw_ver();
    zucmotStatus->cab_status.cabinetID = cab->get_id();
    zucmotStatus->cab_status.currRunTime = cab->get_current_run_time();
    zucmotStatus->cab_status.accuRunTime = cab->get_total_run_time();

    // static double last_vel_cmd[ZUCMOT_MAX_JOINTS];
    static double last_pos_cmd[ZUCMOT_MAX_JOINTS];
    // double joint_acc[ZUCMOT_MAX_JOINTS] = {0};
    // for (joint_num = 0; joint_num < zucmotConfig->numJoints; joint_num++)
    // {
    //     joints[joint_num].vel_cmd = (-last_pos_cmd[joint_num] + joints[joint_num].pos_cmd) / zucmotConfig->trajCycleTime;
    // }

#ifdef WRITE_JOINT_FILE
    if (!file_first)
    {
        joint1_pos = fopen("joint1_pos.txt", "wb");
        joint2_pos = fopen("joint2_pos.txt", "wb");
        joint3_pos = fopen("joint3_pos.txt", "wb");
        joint4_pos = fopen("joint4_pos.txt", "wb");
        joint5_pos = fopen("joint5_pos.txt", "wb");
        joint6_pos = fopen("joint6_pos.txt", "wb");
        joint7_pos = fopen("joint7_pos.txt", "wb");
        joint1_vel = fopen("joint1_vel.txt", "wb");
        joint2_vel = fopen("joint2_vel.txt", "wb");
        joint3_vel = fopen("joint3_vel.txt", "wb");
        joint4_vel = fopen("joint4_vel.txt", "wb");
        joint5_vel = fopen("joint5_vel.txt", "wb");
        joint6_vel = fopen("joint6_vel.txt", "wb");
        joint7_vel = fopen("joint7_vel.txt", "wb");
        consumeTime = fopen("consumeTime.txt", "wb");

        linearVel_file = fopen("linearVel.txt", "wb");
        Angular_Vel_file = fopen("Angular_Vel_file.txt", "wb");
        Attitude_aix = fopen("Attitude_aix.txt", "wb");
        pos_x_file = fopen("pos_x.txt", "wb");
        pos_y_file = fopen("pos_y.txt", "wb");
        pos_z_file = fopen("pos_z.txt", "wb");
        cycle_counter_file = fopen("cycle_counter.txt", "wb");
        if ((!joint2_pos) || (!joint2_vel))
        {
            rtapi_print("update_status : can not open joint2_file  \n");
        }
        if ((!joint3_pos) || (!joint3_vel))
        {
            rtapi_print("update_status : can not open joint3_file  \n");
        }
        if ((!joint4_pos) || (!joint4_vel))
        {
            rtapi_print("update_status : can not open joint4_file  \n");
        }
        if ((!joint5_pos) || (!joint5_vel))
        {
            rtapi_print("update_status : can not open joint5_file  \n");
        }
        if ((!joint6_pos) || (!joint6_vel))
        {
            rtapi_print("update_status : can not open joint6_file  \n");
        }
        if ((!joint7_pos) || (!joint7_vel))
        {
            rtapi_print("update_status : can not open joint6_file  \n");
        }
        if (!consumeTime)
        {
            rtapi_print("update_status : can not open consumeTime_file  \n");
        }
        if (!linearVel_file)
        {
            rtapi_print("update_status : can not open linearVel_file  \n");
        }
        if (!Angular_Vel_file)
        {
            rtapi_print("update_status : can not open Angular_Vel_file  \n");
        }
        if (!Attitude_aix)
        {
            rtapi_print("update_status : can not open Attitude_aix  \n");
        }
        if (!pos_x_file)
        {
            rtapi_print("update_status : can not open pos_x_file  \n");
        }
        if (!pos_y_file)
        {
            rtapi_print("update_status : can not open pos_y_file  \n");
        }
        if (!pos_z_file)
        {
            rtapi_print("update_status : can not open pos_z_file  \n");
        }
        if (!cycle_counter_file)
        {
            rtapi_print("update_status : can not open cycle_counter_file  \n");
        }
        file_first = 1;
    }

    static double last_vel_cmd[ZUCMOT_MAX_JOINTS];
    // static double last_pos_cmd[ZUCMOT_MAX_JOINTS];
    double joint_acc[ZUCMOT_MAX_JOINTS] = {0};
    for (joint_num = 0; joint_num < zucmotConfig->numJoints; joint_num++)
    {
        double jvel_cmd = joints[joint_num].vel_cmd;
        joints[joint_num].vel_cmd = (-last_pos_cmd[joint_num] + joints[joint_num].pos_cmd) / zucmotConfig->trajCycleTime;
        joints[joint_num].acc_cmd = (joints[joint_num].vel_cmd - jvel_cmd) * servo_freq;
        // joint_acc[joint_num] = (-last_vel_cmd[joint_num] + joints[joint_num].vel_cmd) / zucmotConfig->trajCycleTime;
    }

#ifdef WRITE_JOINT_FILE
    int write = 1;
    int j = 1;
    static int writeCount;
    writeCount++;
    //	rtapi_print("[WRITE_JOINT_FILE] : -------------%d-------------\n",writeCount);
    //	rtapi_print("WRITE_JOINT_FILE: --------joints-> pos_cmd %f, %f, %f, %f, %f, %f\n", joints[0].pos_cmd, joints[1].pos_cmd, joints[2].pos_cmd, joints[3].pos_cmd, joints[4].pos_cmd, joints[5].pos_cmd);
    if (joint1_pos)
    {
        fprintf(joint1_pos, "%lf\n", joints[0].pos_cmd);
    }
    if (joint1_vel)
    {
        fprintf(joint1_vel, "%lf\n", joints[0].vel_cmd);
    }
    if (joint2_pos)
    {
        fprintf(joint2_pos, "%lf\n", joints[1].pos_cmd);
    }
    if (joint2_vel)
    {
        fprintf(joint2_vel, "%lf\n", joints[1].vel_cmd);
    }
    if (joint3_pos)
    {
        fprintf(joint3_pos, "%lf\n", joints[2].pos_cmd);
    }
    if (joint3_vel)
    {
        fprintf(joint3_vel, "%lf\n", joints[2].vel_cmd);
    }
    if (joint4_pos)
    {
        fprintf(joint4_pos, "%lf\n", joints[3].pos_cmd);
    }
    if (joint4_vel)
    {
        fprintf(joint4_vel, "%lf\n", joints[3].vel_cmd);
    }
    if (joint5_pos)
    {
        fprintf(joint5_pos, "%lf\n", joints[4].pos_cmd);
    }
    if (joint5_vel)
    {
        fprintf(joint5_vel, "%lf\n", joints[4].vel_cmd);
    }
    if (joint6_pos)
    {
        fprintf(joint6_pos, "%lf\n", joints[5].pos_cmd);
    }
    if (joint6_vel)
    {
        fprintf(joint6_vel, "%lf\n", joints[5].vel_cmd);
    }
    if (joint7_pos)
    {
        fprintf(joint7_pos, "%lf\n", joints[6].pos_cmd);
    }
    if (joint7_vel)
    {
        fprintf(joint7_vel, "%lf\n", joints[6].vel_cmd);
    }
    // static int write_num;
    cycleCount++;
    // rtapi_print("sxl------------------------------------[cycleCount = %d,  joint2_vel = %f]------------------------------------\n", cycleCount, joints[1].vel_cmd);
    // rtapi_print("sxl*******joint2_vel = %f*******\n", joints[1].vel_cmd);

#endif

    // double linear_vel;
    double dx, dy, dz, ds;
    double angular_vel;
    double period = zucmotConfig->trajCycleTime;
    dx = last_carte_pos_cmd.tran.x - zucmotStatus->carte_pos_cmd.tran.x;
    dy = last_carte_pos_cmd.tran.y - zucmotStatus->carte_pos_cmd.tran.y;
    dz = last_carte_pos_cmd.tran.z - zucmotStatus->carte_pos_cmd.tran.z;
    ds = sqrt(dx * dx + dy * dy + dz * dz);
    PmRpy rpycmd, rpycmd_last;
    PmQuaternion quatcmd, quatcmd_last;
    PmRotationVector delta_rot_axis;
    rpycmd.y = zucmotStatus->carte_pos_cmd.a * PM_PI / 180.0;
    rpycmd.p = zucmotStatus->carte_pos_cmd.b * PM_PI / 180.0;
    rpycmd.r = zucmotStatus->carte_pos_cmd.c * PM_PI / 180.0;
    rpycmd_last.y = last_carte_pos_cmd.a * PM_PI / 180.0;
    rpycmd_last.p = last_carte_pos_cmd.b * PM_PI / 180.0;
    rpycmd_last.r = last_carte_pos_cmd.c * PM_PI / 180.0;
    pmRpyQuatConvert(&rpycmd, &quatcmd);
    pmRpyQuatConvert(&rpycmd_last, &quatcmd_last);
    Quaternions_interpolation_angular_vel_calculation(&quatcmd_last, &quatcmd, period, &angular_vel, &delta_rot_axis);
    if (linearVel_file)
    {
        // fprintf(linearVel_file,
        //         "%lf %lf %lf %lf %lf %lf %lf\n",
        //         ds / zucmotConfig->trajCycleTime,
        //         toppra_tor[0],
        //         toppra_tor[1],
        //         toppra_tor[2],
        //         toppra_tor[3],
        //         toppra_tor[4],
        //         toppra_tor[5]);
        fprintf(linearVel_file, "%lf\n", ds / zucmotConfig->trajCycleTime);
    }
    if (Angular_Vel_file)
    {
        fprintf(Angular_Vel_file, "%lf\n", angular_vel);
    }
    if (Attitude_aix)
    {
        fprintf(Attitude_aix, "%lf %lf %lf\n", delta_rot_axis.x, delta_rot_axis.y, delta_rot_axis.z);
    }
    if (pos_x_file)
    {
        fprintf(pos_x_file,
                "%lf %lf %lf %lf %lf %lf %lf\n",
                zucmotStatus->carte_pos_cmd.tran.x,
                toppra_tor[0],
                toppra_tor[1],
                toppra_tor[2],
                toppra_tor[3],
                toppra_tor[4],
                toppra_tor[5]);
    }
    if (pos_y_file)
    {
        fprintf(pos_y_file, "%lf\n", zucmotStatus->carte_pos_cmd.tran.y);
    }
    if (pos_z_file)
    {
        fprintf(pos_z_file, "%lf\n", zucmotStatus->carte_pos_cmd.tran.z);
    }
    if (cycle_counter_file)
    {
        //        if (!tpManager_tpIsDone())
        {
            //            fprintf(cycle_counter_file, "%ld\n", tpManager_GetMotId(0));
        }
        else { fprintf(cycle_counter_file, "%lf\n", pos_error); }
    }
#endif

    for (joint_num = 0; joint_num < zucmotConfig->numJoints; joint_num++)
    {
        last_pos_cmd[joint_num] = joints[joint_num].pos_cmd;
        // last_vel_cmd[joint_num] = joints[joint_num].vel_cmd;
    }

    /* copy status info from private joint structure to status
       struct in shared memory */
    for (joint_num = 0; joint_num < zucmotConfig->numJoints; joint_num++)
    {
        joint = &joints[joint_num];
        joint_status = &(zucmotStatus->joint_status[joint_num]);
        joint_status->flag = joint->flag;
        joint_status->pos_cmd = joint->pos_cmd;
        joint_status->pos_fb = joint->pos_fb;
        joint_status->vel_cmd = joint->vel_cmd;
        joint_status->acc_cmd = joint->acc_cmd;
        joint_status->ferror = joint->ferror;
        joint_status->ferror_high_mark = joint->ferror_high_mark;
        joint_status->backlash = joint->backlash;
        joint_status->max_pos_limit = joint->max_pos_limit;
        joint_status->min_pos_limit = joint->min_pos_limit;
        joint_status->min_ferror = joint->min_ferror;
        joint_status->max_ferror = joint->max_ferror;
        joint_status->home_offset = joint->home_offset;
        joint_status->vel_limit = joint->vel_limit;
        joint_status->acc_limit = joint->acc_limit;
        // joint_status->servo_warn_8384_cnt = getServoErrStatistic(joint_num, DRIVE_WARN_ENCODE_CNT).cntAll;
        // joint_status->servo_warn_8385_cnt = getServoErrStatistic(joint_num, DRIVE_WARN_ENCODE_DUAL_CHECK).cntAll;
        // joint_status->servo_warn_8387_cnt = getServoErrStatistic(joint_num, DRIVE_WARN_ENCODE_Z).cntAll;
        // joint_status->servo_warn_8388_cnt = getServoErrStatistic(joint_num, DRIVE_WARN_ENCODE_SIGNAL).cntAll;
        // joint_status->servo_warn_7001_cnt = getServoErrStatistic(joint_num, DRIVE_WARN_ENCODE_INNER).cntAll;
    }

    for (axis_num = 0; axis_num < ZUCMOT_MAX_AXIS; axis_num++)
    {
        axis = &axes[axis_num];
        axis_status = &(zucmotStatus->axis_status[axis_num]);
        axis_status->teleop_vel_cmd = axis->teleop_vel_cmd;
        axis_status->teleop_tool_vel_cmd = axis->teleop_tool_vel_cmd;
        axis_status->max_pos_limit = axis->max_pos_limit;
        axis_status->min_pos_limit = axis->min_pos_limit;
    }

    check_capture();

    update_mb_IO();
    update_phy_IO();

    /* synchronize the extended IO status*/
    zucmotStatus->extio_stat = zucmotExtIOStruct->extIOShmStruct.extIOStat;
    /* synchronize the tool IO status*/
    //
    zucmotStatus->tio_status = tioStatus;
    //  physic io status to modbus
    get_physic_IO_status();

    /* synchronize the gripper signal status*/
    GripperStateUnit* grrpperState = tio_gripper_list_signal();
    for (i = 0; i < MAX_TIO_SIG_NUM; i++) { zucmotStatus->gripperState[i] = grrpperState[i]; }

    /* synchronize the sensor status*/
    zucmotStatus->sensor_stat = zucmotSensorStruct->sensorStatus;
    oldId = zucmotStatus->id;
    /* motion zucmotDebug coord_tp status */
    //    zucmotStatus->depth = tpManager_tpQueueDepth();
    //    zucmotStatus->activeDepth = tpManager_tpActiveDepth();
    //    zucmotStatus->id = tpManager_tpGetExecId();
    //    zucmotStatus->motionType = tpManager_tpGetMotionType();
    //    zucmotStatus->queueFull = tpManager_tcqFull();

    temp_var = 1;
    for (joint_num = 0; joint_num < zucmotConfig->numJoints; joint_num++)
    {
        if (fabs(joints[joint_num].pos_cmd - zucmotConfig->safetyCfg.init_traj_jpos[joint_num]) > zucmotConfig->safetyCfg.init_jpos_tol)
        {
            temp_var = 0;
            break;
        }
    }
    zucmotStatus->in_home_position = temp_var;

    /* check to see if we should pause in order to implement
       single zucmotDebug->stepping */

    if (zucmotDebug->stepping && ((zucmotDebug->idForStep != 0 && zucmotDebug->idForStep != zucmotStatus->id) || (oldId != 0 && oldId != zucmotStatus->id)))
    {
        //        tpManager_tppause();
        zucmotDebug->stepping = 0;
        zucmotStatus->paused = 1;
        // rtapi_print("zucmotDebug->stepping = %d, zucmotDebug->idForStep=%d, zucmotStatus->id= %d, oldId = %d, zucmotStatus->depth = %d\n",
        //             zucmotDebug->stepping,
        //             zucmotDebug->idForStep,
        //             zucmotStatus->id,
        //             oldId,
        //             zucmotStatus->depth);
    }

    auto scbio = rtdev::RtDevMgr::instance().get_scb()->io();
    if (zucmotConfig->cab_type == 2)
    {
        zucmotStatus->scb_anachn_setting = scbio->get_aio_type(0);
    }
    else if (zucmotConfig->cab_type == 3)
    {
        zucmotStatus->scb_digchn_setting = scbio->get_dio_type(0);
        zucmotStatus->scb_br_start_vol = rtdev::RtDevMgr::instance().get_scb()->cab()->get_stuck_voltage();
        auto br_start_vol_cmd = rtdev::RtDevMgr::instance().get_scb()->cab()->get_stuck_voltage(false);

        if ((zucmotStatus->after_set_scb_br_start_vol_flag == 50) && (zucmotStatus->scb_br_start_vol != br_start_vol_cmd) &&
            (zucmotStatus->set_scb_br_start_vol_flag == 1))
        {
            reportWarning(SCB_ERR_OUT_BODY_SUPPLY_ABNORMAL, "scb error out body supply abnormal");
            zucmotStatus->set_scb_br_start_vol_flag = 0;
        }
    }

    if (zucmotConfig->safetyCfg.motRunMode == RUN_MODE_SIM_WITHOUT_CAN || zucmotConfig->safetyCfg.motRunMode == RUN_MODE_SIM_ON_REAL)
    {
        zucmotStatus->is_powering_on = zucmotStatus->powered_on;
    }
    else
    {
        //FIXME:xxf 使用axisgroup 状态机接口判断
        zucmotStatus->is_powering_on = zucmotStatus->powered_on;  //rtdev::get_scb()->get_scb_power_robot_req();
    }

    zucmotStatus->is_being_enabled = zucmotDebug->enabling;

    if ((zucmot_hal_data->zuc_enable_in) != 0)
    {
        // if (zucmotStatus->servoDHState != get_dhparam_state(NULL))
        // {
        //     zucmotStatus->servoDHState = get_dhparam_state(NULL);
        //     if (zucmotStatus->servoDHState == 2)
        //     {
        //         rtapi_print("DH in servo is ready: control.c\n");
        //         get_dhparam_state(&zucmotConfig->dhParam);
        //     }
        // }
        // if (zucmotStatus->servoDynState != get_dynparam_state(NULL))
        // {
        //     zucmotStatus->servoDynState = get_dynparam_state(NULL);
        //     if (zucmotStatus->servoDynState == 2)
        //     {
        //         rtapi_print("Dyn in servo is ready: control.c, %d\n", zucmotStatus->servoDynState);
        //         get_dynparam_state(&zucmotConfig->dynParam);
        //     }
        // }
        if (wait_robot_power_on != 0)
        {
            if ((zucmotStatus->servoDHState == 1 /* Not enabled */ || zucmotStatus->servoDHState == 2 ||
                 zucmotStatus->servoDHState == 3 /* CRC check failed */) &&
                (zucmotStatus->servoDynState == 1 /* Not enabled */ || zucmotStatus->servoDynState == 2 ||
                 zucmotStatus->servoDynState == 3 /* CRC check failed */))
            {
                zucmotStatus->powered_on = 1;
                // zucmotStatus->is_powering_on = 0;  //上电成功，reset 正在上电标记
                (zucmot_hal_data->robot_powered_on) = 1;
                wait_robot_power_on = 0;
                rtapi_print(">>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>robot powered on,  zucmotStatus->servoDynState = %d,  zucmotStatus->servoDHState = %d",
                            zucmotStatus->servoDynState,
                            zucmotStatus->servoDHState);
            }
            else
            {
                wait_robot_power_on++;
            }

            if (wait_robot_power_on == TIMER_20S)
            {
#if 0
                // *(zucmot_hal_data->zuc_enable_cmd) = 0;
                wait_robot_power_on = 0;
                reportError(ROBOT_POWER_ON_TIMEOUT, "robot power on timeout");
#else
                zucmotStatus->servoDHState = 1;
                zucmotStatus->servoDynState = 1;
                zucmotStatus->powered_on = 1;
                // zucmotStatus->is_powering_on = 0;  //上电成功，reset 正在上电标记
                (zucmot_hal_data->robot_powered_on) = 1;
                wait_robot_power_on = 0;
                rtapi_print(">>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>robot powered on,  zucmotStatus->servoDynState = %d,  zucmotStatus->servoDHState = %d",
                            zucmotStatus->servoDynState,
                            zucmotStatus->servoDHState);
#endif
            }
        }
    }
    else
    {
        zucmotStatus->servoDHState = 0;
        zucmotStatus->servoDynState = 0;
    }
    //    tpManager_GetcurSegTargetPose(&zucmotStatus->curSegTargetPose);
    // if(zucmotStatus->powered_on && last_ioaio_conn && !*(zucmot_hal_data->ioaio_conn)) {
    // 	rtapi_print("ioaio connection errror, please check\n");
    // 	*(zucmot_hal_data->zuc_enable_cmd) = 0; // power off
    // }
    // last_ioaio_conn = *(zucmot_hal_data->ioaio_conn);
}

static int posCmdInRange(double* joint_cmds, double* curr_joints, int* err_jnt_num)
{
    int res = 1, jnt_num = 0;
    *err_jnt_num = -1;
    for (jnt_num = 0; jnt_num < zucmotConfig->numJoints; ++jnt_num)
    {
        if (fabs(joint_cmds[jnt_num] - curr_joints[jnt_num]) >= 180)
        {
            res = 0;
            *err_jnt_num = jnt_num;
            break;
        }
    }
    return res;
}

/*check any joint in fault or fault reaction*/
static bool any_joint_in_fault(void)
{
    bool res = false;
    // for (int i = 0; i < zucmotConfig->numJoints; ++i) res = res || servoStatus[i].errorState;
    return res;
}

/*check any unhandled error in each joint*/
static bool any_unhandled_error(void)
{
    bool res = false;
    // for (int i = 0; i < zucmotConfig->numJoints; ++i) res = res || (servoStatus[i].errorCode != 0);
    return res;
}

static int check_poweron_stat(int poweron, int enable)
{
    static int powerStateCnt = 0;
    int ret = 0;
    if (enable)
        return 1;
    if (poweron == 1 && powerStateCnt < 3000)
    {
        powerStateCnt++;
    }
    else if (poweron == 1 && powerStateCnt == 3000)
    {
        ret = 1;
    }
    else
    {
        powerStateCnt = 0;
    }
    return ret;
}

static void set_bit(int* flag, int bit) { *flag |= bit; }

static void clear_bit(int* flag, int bit) { *flag &= ~bit; }

//return 1 if bit is set
static bool check_bit(int* flag, int bit) { return (*flag & bit) != 0; }
